projects / GitHub / mt8127 / android_kernel_alcatel_ttab.git / blame
? search:
summary | shortlog | log | commit | commitdiff | tree
blob | blame (incremental) | history | HEAD
Bluetooth: Use queue in the device list
[GitHub/mt8127/android_kernel_alcatel_ttab.git] / net / bluetooth / l2cap_core.c
CommitLineData
8e87d142 1/*
1da177e4
LT		 2 BlueZ - Bluetooth protocol stack for Linux
3 Copyright (C) 2000-2001 Qualcomm Incorporated
ce5706bd 4 Copyright (C) 2009-2010 Gustavo F. Padovan <gustavo@padovan.org>
5d8868ff 5 Copyright (C) 2010 Google Inc.
1da177e4
LT		 6
7 Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.com>
8
9 This program is free software; you can redistribute it and/or modify
10 it under the terms of the GNU General Public License version 2 as
11 published by the Free Software Foundation;
12
13 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
14 OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS.
16 IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY
8e87d142
YH		 17 CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES
18 WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
19 ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
1da177e4
LT		 20 OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
21
8e87d142
YH		 22 ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS,
23 COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS
1da177e4
LT		 24 SOFTWARE IS DISCLAIMED.
25*/
26
bb58f747 27/* Bluetooth L2CAP core. */
1da177e4 28
1da177e4
LT		 29#include <linux/module.h>
30
31#include <linux/types.h>
4fc268d2 32#include <linux/capability.h>
1da177e4
LT		 33#include <linux/errno.h>
34#include <linux/kernel.h>
1da177e4
LT		 35#include <linux/sched.h>
36#include <linux/slab.h>
37#include <linux/poll.h>
38#include <linux/fcntl.h>
39#include <linux/init.h>
40#include <linux/interrupt.h>
41#include <linux/socket.h>
42#include <linux/skbuff.h>
1da177e4 43#include <linux/list.h>
be9d1227 44#include <linux/device.h>
aef7d97c
MH		 45#include <linux/debugfs.h>
46#include <linux/seq_file.h>
af05b30b 47#include <linux/uaccess.h>
fcc203c3 48#include <linux/crc16.h>
1da177e4
LT		 49#include <net/sock.h>
50
51#include <asm/system.h>
1da177e4
LT		 52#include <asm/unaligned.h>
53
54#include <net/bluetooth/bluetooth.h>
55#include <net/bluetooth/hci_core.h>
56#include <net/bluetooth/l2cap.h>
b501d6a1 57#include <net/bluetooth/smp.h>
1da177e4 58
bb58f747 59int disable_ertm;
f0709e03 60
47ec1dcd 61static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN;
50a147cd 62static u8 l2cap_fixed_chan[8] = { L2CAP_FC_L2CAP, };
1da177e4 63
b5ad8b7f
JB		 64static LIST_HEAD(chan_list);
65static DEFINE_RWLOCK(chan_list_lock);
1da177e4 66
1da177e4
LT		 67static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
68 u8 code, u8 ident, u16 dlen, void *data);
4519de9a
GP		 69static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len,
70 void *data);
710f9b0a 71static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data);
4519de9a
GP		 72static void l2cap_send_disconn_req(struct l2cap_conn *conn,
73 struct l2cap_chan *chan, int err);
1da177e4 74
218bb9df
GP		 75static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb);
76
0139418c 77/* ---- L2CAP channels ---- */
71ba0e56
GP		 78
79static inline void chan_hold(struct l2cap_chan *c)
80{
81 atomic_inc(&c->refcnt);
82}
83
84static inline void chan_put(struct l2cap_chan *c)
85{
86 if (atomic_dec_and_test(&c->refcnt))
87 kfree(c);
88}
89
baa7e1fa 90static struct l2cap_chan *__l2cap_get_chan_by_dcid(struct l2cap_conn *conn, u16 cid)
0139418c 91{
48454079 92 struct l2cap_chan *c;
baa7e1fa
GP		 93
94 list_for_each_entry(c, &conn->chan_l, list) {
fe4128e0 95 if (c->dcid == cid)
baa7e1fa 96 return c;
0139418c 97 }
baa7e1fa 98 return NULL;
0139418c
MH		 99}
100
baa7e1fa 101static struct l2cap_chan *__l2cap_get_chan_by_scid(struct l2cap_conn *conn, u16 cid)
0139418c 102{
48454079 103 struct l2cap_chan *c;
baa7e1fa
GP		 104
105 list_for_each_entry(c, &conn->chan_l, list) {
fe4128e0 106 if (c->scid == cid)
baa7e1fa 107 return c;
0139418c 108 }
baa7e1fa 109 return NULL;
0139418c
MH		 110}
111
112/* Find channel with given SCID.
113 * Returns locked socket */
baa7e1fa 114static struct l2cap_chan *l2cap_get_chan_by_scid(struct l2cap_conn *conn, u16 cid)
0139418c 115{
48454079 116 struct l2cap_chan *c;
baa7e1fa
GP		 117
118 read_lock(&conn->chan_lock);
119 c = __l2cap_get_chan_by_scid(conn, cid);
48454079
GP		 120 if (c)
121 bh_lock_sock(c->sk);
baa7e1fa 122 read_unlock(&conn->chan_lock);
48454079 123 return c;
0139418c
MH		 124}
125
baa7e1fa 126static struct l2cap_chan *__l2cap_get_chan_by_ident(struct l2cap_conn *conn, u8 ident)
0139418c 127{
48454079 128 struct l2cap_chan *c;
baa7e1fa
GP		 129
130 list_for_each_entry(c, &conn->chan_l, list) {
fc7f8a7e 131 if (c->ident == ident)
baa7e1fa 132 return c;
0139418c 133 }
baa7e1fa 134 return NULL;
0139418c
MH		 135}
136
baa7e1fa 137static inline struct l2cap_chan *l2cap_get_chan_by_ident(struct l2cap_conn *conn, u8 ident)
0139418c 138{
48454079 139 struct l2cap_chan *c;
baa7e1fa
GP		 140
141 read_lock(&conn->chan_lock);
142 c = __l2cap_get_chan_by_ident(conn, ident);
48454079
GP		 143 if (c)
144 bh_lock_sock(c->sk);
baa7e1fa 145 read_unlock(&conn->chan_lock);
48454079 146 return c;
0139418c
MH		 147}
148
23691d75 149static struct l2cap_chan *__l2cap_global_chan_by_addr(__le16 psm, bdaddr_t *src)
9e4425ff 150{
23691d75 151 struct l2cap_chan *c;
9e4425ff 152
23691d75
GP		 153 list_for_each_entry(c, &chan_list, global_l) {
154 if (c->sport == psm && !bacmp(&bt_sk(c->sk)->src, src))
250938cb 155 return c;
9e4425ff 156 }
250938cb 157 return NULL;
9e4425ff
GP		 158}
159
160int l2cap_add_psm(struct l2cap_chan *chan, bdaddr_t *src, __le16 psm)
161{
73b2ec18
GP		 162 int err;
163
23691d75 164 write_lock_bh(&chan_list_lock);
9e4425ff 165
23691d75 166 if (psm && __l2cap_global_chan_by_addr(psm, src)) {
73b2ec18
GP		 167 err = -EADDRINUSE;
168 goto done;
9e4425ff
GP		 169 }
170
73b2ec18
GP		 171 if (psm) {
172 chan->psm = psm;
173 chan->sport = psm;
174 err = 0;
175 } else {
176 u16 p;
177
178 err = -EINVAL;
179 for (p = 0x1001; p < 0x1100; p += 2)
23691d75 180 if (!__l2cap_global_chan_by_addr(cpu_to_le16(p), src)) {
73b2ec18
GP		 181 chan->psm = cpu_to_le16(p);
182 chan->sport = cpu_to_le16(p);
183 err = 0;
184 break;
185 }
186 }
9e4425ff 187
73b2ec18 188done:
23691d75 189 write_unlock_bh(&chan_list_lock);
73b2ec18 190 return err;
9e4425ff
GP		 191}
192
193int l2cap_add_scid(struct l2cap_chan *chan, __u16 scid)
194{
23691d75 195 write_lock_bh(&chan_list_lock);
9e4425ff
GP		 196
197 chan->scid = scid;
198
23691d75 199 write_unlock_bh(&chan_list_lock);
9e4425ff
GP		 200
201 return 0;
202}
203
baa7e1fa 204static u16 l2cap_alloc_cid(struct l2cap_conn *conn)
0139418c 205{
8db4dc46 206 u16 cid = L2CAP_CID_DYN_START;
0139418c 207
8db4dc46 208 for (; cid < L2CAP_CID_DYN_END; cid++) {
baa7e1fa 209 if (!__l2cap_get_chan_by_scid(conn, cid))
0139418c
MH		 210 return cid;
211 }
212
213 return 0;
214}
215
c9b66675 216static void l2cap_set_timer(struct l2cap_chan *chan, struct timer_list *timer, long timeout)
ab07801d 217{
457f4850 218 BT_DBG("chan %p state %d timeout %ld", chan, chan->state, timeout);
89bc500e 219
942ecc9c 220 if (!mod_timer(timer, jiffies + msecs_to_jiffies(timeout)))
774e5651 221 chan_hold(chan);
ab07801d
GP		 222}
223
c9b66675 224static void l2cap_clear_timer(struct l2cap_chan *chan, struct timer_list *timer)
ab07801d 225{
774e5651 226 BT_DBG("chan %p state %d", chan, chan->state);
ab07801d 227
774e5651
MM		 228 if (timer_pending(timer) && del_timer(timer))
229 chan_put(chan);
ab07801d
GP		 230}
231
89bc500e
GP		 232static void l2cap_state_change(struct l2cap_chan *chan, int state)
233{
234 chan->state = state;
235 chan->ops->state_change(chan->data, state);
236}
237
ab07801d
GP		 238static void l2cap_chan_timeout(unsigned long arg)
239{
240 struct l2cap_chan *chan = (struct l2cap_chan *) arg;
241 struct sock *sk = chan->sk;
242 int reason;
243
89bc500e 244 BT_DBG("chan %p state %d", chan, chan->state);
ab07801d
GP		 245
246 bh_lock_sock(sk);
247
248 if (sock_owned_by_user(sk)) {
249 /* sk is owned by user. Try again later */
f3f668b0 250 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
ab07801d 251 bh_unlock_sock(sk);
71ba0e56 252 chan_put(chan);
ab07801d
GP		 253 return;
254 }
255
89bc500e 256 if (chan->state == BT_CONNECTED || chan->state == BT_CONFIG)
ab07801d 257 reason = ECONNREFUSED;
89bc500e 258 else if (chan->state == BT_CONNECT &&
ab07801d
GP		 259 chan->sec_level != BT_SECURITY_SDP)
260 reason = ECONNREFUSED;
261 else
262 reason = ETIMEDOUT;
263
0f852724 264 l2cap_chan_close(chan, reason);
ab07801d
GP		 265
266 bh_unlock_sock(sk);
267
ba3bd0ee 268 chan->ops->close(chan->data);
71ba0e56 269 chan_put(chan);
ab07801d
GP		 270}
271
23691d75 272struct l2cap_chan *l2cap_chan_create(struct sock *sk)
48454079
GP		 273{
274 struct l2cap_chan *chan;
275
276 chan = kzalloc(sizeof(*chan), GFP_ATOMIC);
277 if (!chan)
278 return NULL;
279
280 chan->sk = sk;
281
23691d75
GP		 282 write_lock_bh(&chan_list_lock);
283 list_add(&chan->global_l, &chan_list);
284 write_unlock_bh(&chan_list_lock);
285
ab07801d
GP		 286 setup_timer(&chan->chan_timer, l2cap_chan_timeout, (unsigned long) chan);
287
89bc500e
GP		 288 chan->state = BT_OPEN;
289
71ba0e56
GP		 290 atomic_set(&chan->refcnt, 1);
291
abc545b8
SJ		 292 BT_DBG("sk %p chan %p", sk, chan);
293
48454079
GP		 294 return chan;
295}
296
23691d75 297void l2cap_chan_destroy(struct l2cap_chan *chan)
6ff5abbf 298{
23691d75
GP		 299 write_lock_bh(&chan_list_lock);
300 list_del(&chan->global_l);
301 write_unlock_bh(&chan_list_lock);
302
71ba0e56 303 chan_put(chan);
6ff5abbf
GP		 304}
305
48454079 306static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
0139418c 307{
af05b30b 308 BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn,
fe4128e0 309 chan->psm, chan->dcid);
0139418c 310
9f5a0d7b 311 conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM;
2950f21a 312
8c1d787b 313 chan->conn = conn;
0139418c 314
715ec005 315 if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED) {
b62f328b
VT		 316 if (conn->hcon->type == LE_LINK) {
317 /* LE connection */
0c1bc5c6 318 chan->omtu = L2CAP_LE_DEFAULT_MTU;
fe4128e0
GP		 319 chan->scid = L2CAP_CID_LE_DATA;
320 chan->dcid = L2CAP_CID_LE_DATA;
b62f328b
VT		 321 } else {
322 /* Alloc CID for connection-oriented socket */
fe4128e0 323 chan->scid = l2cap_alloc_cid(conn);
0c1bc5c6 324 chan->omtu = L2CAP_DEFAULT_MTU;
b62f328b 325 }
715ec005 326 } else if (chan->chan_type == L2CAP_CHAN_CONN_LESS) {
0139418c 327 /* Connectionless socket */
fe4128e0
GP		 328 chan->scid = L2CAP_CID_CONN_LESS;
329 chan->dcid = L2CAP_CID_CONN_LESS;
0c1bc5c6 330 chan->omtu = L2CAP_DEFAULT_MTU;
0139418c
MH		 331 } else {
332 /* Raw socket can send/recv signalling messages only */
fe4128e0
GP		 333 chan->scid = L2CAP_CID_SIGNALING;
334 chan->dcid = L2CAP_CID_SIGNALING;
0c1bc5c6 335 chan->omtu = L2CAP_DEFAULT_MTU;
0139418c
MH		 336 }
337
8f7975b1
AE		 338 chan->local_id = L2CAP_BESTEFFORT_ID;
339 chan->local_stype = L2CAP_SERV_BESTEFFORT;
340 chan->local_msdu = L2CAP_DEFAULT_MAX_SDU_SIZE;
341 chan->local_sdu_itime = L2CAP_DEFAULT_SDU_ITIME;
342 chan->local_acc_lat = L2CAP_DEFAULT_ACC_LAT;
343 chan->local_flush_to = L2CAP_DEFAULT_FLUSH_TO;
344
71ba0e56 345 chan_hold(chan);
baa7e1fa
GP		 346
347 list_add(&chan->list, &conn->chan_l);
0139418c
MH		 348}
349
8e87d142 350/* Delete channel.
0139418c 351 * Must be called on the locked socket. */
4519de9a 352static void l2cap_chan_del(struct l2cap_chan *chan, int err)
0139418c 353{
48454079 354 struct sock *sk = chan->sk;
8c1d787b 355 struct l2cap_conn *conn = chan->conn;
0139418c
MH		 356 struct sock *parent = bt_sk(sk)->parent;
357
c9b66675 358 __clear_chan_timer(chan);
0139418c 359
49208c9c 360 BT_DBG("chan %p, conn %p, err %d", chan, conn, err);
0139418c 361
8e87d142 362 if (conn) {
baa7e1fa
GP		 363 /* Delete from channel list */
364 write_lock_bh(&conn->chan_lock);
365 list_del(&chan->list);
366 write_unlock_bh(&conn->chan_lock);
71ba0e56 367 chan_put(chan);
baa7e1fa 368
8c1d787b 369 chan->conn = NULL;
0139418c
MH		 370 hci_conn_put(conn->hcon);
371 }
372
89bc500e 373 l2cap_state_change(chan, BT_CLOSED);
0139418c
MH		 374 sock_set_flag(sk, SOCK_ZAPPED);
375
376 if (err)
377 sk->sk_err = err;
378
379 if (parent) {
380 bt_accept_unlink(sk);
381 parent->sk_data_ready(parent, 0);
382 } else
383 sk->sk_state_change(sk);
c13ffa62 384
c1360a1c
GP		 385 if (!(test_bit(CONF_OUTPUT_DONE, &chan->conf_state) &&
386 test_bit(CONF_INPUT_DONE, &chan->conf_state)))
6ff5abbf 387 return;
2ead70b8 388
58d35f87 389 skb_queue_purge(&chan->tx_q);
c13ffa62 390
0c1bc5c6 391 if (chan->mode == L2CAP_MODE_ERTM) {
c13ffa62
GP		 392 struct srej_list *l, *tmp;
393
1a09bcb9
GP		 394 __clear_retrans_timer(chan);
395 __clear_monitor_timer(chan);
396 __clear_ack_timer(chan);
c13ffa62 397
f1c6775b 398 skb_queue_purge(&chan->srej_q);
c13ffa62 399
39d5a3ee 400 list_for_each_entry_safe(l, tmp, &chan->srej_l, list) {
c13ffa62
GP		 401 list_del(&l->list);
402 kfree(l);
403 }
404 }
0139418c
MH		 405}
406
4519de9a
GP		 407static void l2cap_chan_cleanup_listen(struct sock *parent)
408{
409 struct sock *sk;
410
411 BT_DBG("parent %p", parent);
412
413 /* Close not yet accepted channels */
0f852724 414 while ((sk = bt_accept_dequeue(parent, NULL))) {
ba3bd0ee 415 struct l2cap_chan *chan = l2cap_pi(sk)->chan;
c9b66675 416 __clear_chan_timer(chan);
0f852724 417 lock_sock(sk);
ba3bd0ee 418 l2cap_chan_close(chan, ECONNRESET);
0f852724 419 release_sock(sk);
ba3bd0ee 420 chan->ops->close(chan->data);
0f852724 421 }
4519de9a
GP		 422}
423
0f852724 424void l2cap_chan_close(struct l2cap_chan *chan, int reason)
4519de9a
GP		 425{
426 struct l2cap_conn *conn = chan->conn;
427 struct sock *sk = chan->sk;
428
89bc500e 429 BT_DBG("chan %p state %d socket %p", chan, chan->state, sk->sk_socket);
4519de9a 430
89bc500e 431 switch (chan->state) {
4519de9a
GP		 432 case BT_LISTEN:
433 l2cap_chan_cleanup_listen(sk);
89bc500e
GP		 434
435 l2cap_state_change(chan, BT_CLOSED);
436 sock_set_flag(sk, SOCK_ZAPPED);
4519de9a
GP		 437 break;
438
439 case BT_CONNECTED:
440 case BT_CONFIG:
715ec005 441 if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED &&
4519de9a 442 conn->hcon->type == ACL_LINK) {
c9b66675
GP		 443 __clear_chan_timer(chan);
444 __set_chan_timer(chan, sk->sk_sndtimeo);
4519de9a
GP		 445 l2cap_send_disconn_req(conn, chan, reason);
446 } else
447 l2cap_chan_del(chan, reason);
448 break;
449
450 case BT_CONNECT2:
715ec005 451 if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED &&
4519de9a
GP		 452 conn->hcon->type == ACL_LINK) {
453 struct l2cap_conn_rsp rsp;
454 __u16 result;
455
456 if (bt_sk(sk)->defer_setup)
457 result = L2CAP_CR_SEC_BLOCK;
458 else
459 result = L2CAP_CR_BAD_PSM;
89bc500e 460 l2cap_state_change(chan, BT_DISCONN);
4519de9a
GP		 461
462 rsp.scid = cpu_to_le16(chan->dcid);
463 rsp.dcid = cpu_to_le16(chan->scid);
464 rsp.result = cpu_to_le16(result);
465 rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
466 l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
467 sizeof(rsp), &rsp);
468 }
469
470 l2cap_chan_del(chan, reason);
471 break;
472
473 case BT_CONNECT:
474 case BT_DISCONN:
475 l2cap_chan_del(chan, reason);
476 break;
477
478 default:
479 sock_set_flag(sk, SOCK_ZAPPED);
480 break;
481 }
482}
483
4343478f 484static inline u8 l2cap_get_auth_type(struct l2cap_chan *chan)
79d554a6 485{
715ec005 486 if (chan->chan_type == L2CAP_CHAN_RAW) {
4343478f 487 switch (chan->sec_level) {
8556edd3
JH		 488 case BT_SECURITY_HIGH:
489 return HCI_AT_DEDICATED_BONDING_MITM;
490 case BT_SECURITY_MEDIUM:
491 return HCI_AT_DEDICATED_BONDING;
492 default:
493 return HCI_AT_NO_BONDING;
494 }
fe4128e0 495 } else if (chan->psm == cpu_to_le16(0x0001)) {
4343478f
GP		 496 if (chan->sec_level == BT_SECURITY_LOW)
497 chan->sec_level = BT_SECURITY_SDP;
0684e5f9 498
4343478f 499 if (chan->sec_level == BT_SECURITY_HIGH)
8556edd3 500 return HCI_AT_NO_BONDING_MITM;
00ae4af9 501 else
8556edd3 502 return HCI_AT_NO_BONDING;
00ae4af9 503 } else {
4343478f 504 switch (chan->sec_level) {
00ae4af9 505 case BT_SECURITY_HIGH:
8556edd3 506 return HCI_AT_GENERAL_BONDING_MITM;
00ae4af9 507 case BT_SECURITY_MEDIUM:
8556edd3 508 return HCI_AT_GENERAL_BONDING;
00ae4af9 509 default:
8556edd3 510 return HCI_AT_NO_BONDING;
00ae4af9 511 }
0684e5f9 512 }
8556edd3
JH		 513}
514
515/* Service level security */
d45fc423 516int l2cap_chan_check_security(struct l2cap_chan *chan)
8556edd3 517{
8c1d787b 518 struct l2cap_conn *conn = chan->conn;
8556edd3
JH		 519 __u8 auth_type;
520
4343478f 521 auth_type = l2cap_get_auth_type(chan);
79d554a6 522
4343478f 523 return hci_conn_security(conn->hcon, chan->sec_level, auth_type);
79d554a6
MH		 524}
525
b5ad8b7f 526static u8 l2cap_get_ident(struct l2cap_conn *conn)
4e8402a3
MH		 527{
528 u8 id;
529
530 /* Get next available identificator.
531 * 1 - 128 are used by kernel.
532 * 129 - 199 are reserved.
533 * 200 - 254 are used by utilities like l2ping, etc.
534 */
535
536 spin_lock_bh(&conn->lock);
537
538 if (++conn->tx_ident > 128)
539 conn->tx_ident = 1;
540
541 id = conn->tx_ident;
542
543 spin_unlock_bh(&conn->lock);
544
545 return id;
546}
547
4519de9a 548static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, void *data)
4e8402a3
MH		 549{
550 struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data);
e702112f 551 u8 flags;
4e8402a3
MH		 552
553 BT_DBG("code 0x%2.2x", code);
554
555 if (!skb)
9a9c6a34 556 return;
4e8402a3 557
e702112f
AE		 558 if (lmp_no_flush_capable(conn->hcon->hdev))
559 flags = ACL_START_NO_FLUSH;
560 else
561 flags = ACL_START;
562
14b12d0b 563 bt_cb(skb)->force_active = BT_POWER_FORCE_ACTIVE_ON;
5e59b791 564 skb->priority = HCI_PRIO_MAX;
14b12d0b 565
73d80deb
LAD		 566 hci_send_acl(conn->hchan, skb, flags);
567}
568
569static void l2cap_do_send(struct l2cap_chan *chan, struct sk_buff *skb)
570{
571 struct hci_conn *hcon = chan->conn->hcon;
572 u16 flags;
573
574 BT_DBG("chan %p, skb %p len %d priority %u", chan, skb, skb->len,
575 skb->priority);
576
577 if (!test_bit(FLAG_FLUSHABLE, &chan->flags) &&
578 lmp_no_flush_capable(hcon->hdev))
579 flags = ACL_START_NO_FLUSH;
580 else
581 flags = ACL_START;
14b12d0b 582
73d80deb
LAD		 583 bt_cb(skb)->force_active = test_bit(FLAG_FORCE_ACTIVE, &chan->flags);
584 hci_send_acl(chan->conn->hchan, skb, flags);
4e8402a3
MH		 585}
586
88843ab0 587static inline void l2cap_send_sframe(struct l2cap_chan *chan, u32 control)
1c2acffb
GP		 588{
589 struct sk_buff *skb;
590 struct l2cap_hdr *lh;
8c1d787b 591 struct l2cap_conn *conn = chan->conn;
e4ca6d98 592 int count, hlen;
fcc203c3 593
89bc500e 594 if (chan->state != BT_CONNECTED)
c13ffa62
GP		 595 return;
596
e4ca6d98
AE		 597 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
598 hlen = L2CAP_EXT_HDR_SIZE;
599 else
600 hlen = L2CAP_ENH_HDR_SIZE;
601
47d1ec61 602 if (chan->fcs == L2CAP_FCS_CRC16)
03a51213 603 hlen += L2CAP_FCS_SIZE;
1c2acffb 604
88843ab0 605 BT_DBG("chan %p, control 0x%8.8x", chan, control);
1c2acffb 606
fcc203c3 607 count = min_t(unsigned int, conn->mtu, hlen);
793c2f1c
AE		 608
609 control |= __set_sframe(chan);
1c2acffb 610
e2ab4353 611 if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
03f6715d 612 control |= __set_ctrl_final(chan);
9e917af1 613
e2ab4353 614 if (test_and_clear_bit(CONN_SEND_PBIT, &chan->conn_state))
e3781735 615 control |= __set_ctrl_poll(chan);
f0946ccf 616
1c2acffb
GP		 617 skb = bt_skb_alloc(count, GFP_ATOMIC);
618 if (!skb)
9a9c6a34 619 return;
1c2acffb
GP		 620
621 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
fcc203c3 622 lh->len = cpu_to_le16(hlen - L2CAP_HDR_SIZE);
fe4128e0 623 lh->cid = cpu_to_le16(chan->dcid);
88843ab0
AE		 624
625 __put_control(chan, control, skb_put(skb, __ctrl_size(chan)));
1c2acffb 626
47d1ec61 627 if (chan->fcs == L2CAP_FCS_CRC16) {
03a51213
AE		 628 u16 fcs = crc16(0, (u8 *)lh, count - L2CAP_FCS_SIZE);
629 put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE));
fcc203c3
GP		 630 }
631
73d80deb
LAD		 632 skb->priority = HCI_PRIO_MAX;
633 l2cap_do_send(chan, skb);
1c2acffb
GP		 634}
635
88843ab0 636static inline void l2cap_send_rr_or_rnr(struct l2cap_chan *chan, u32 control)
7e743090 637{
e2ab4353 638 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
ab784b73 639 control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR);
e2ab4353 640 set_bit(CONN_RNR_SENT, &chan->conn_state);
1890d36b 641 } else
ab784b73 642 control |= __set_ctrl_super(chan, L2CAP_SUPER_RR);
7e743090 643
0b209fae 644 control |= __set_reqseq(chan, chan->buffer_seq);
2ab25cdd 645
525cd185 646 l2cap_send_sframe(chan, control);
7e743090
GP		 647}
648
b4450035 649static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan)
e501d055 650{
c1360a1c 651 return !test_bit(CONF_CONNECT_PEND, &chan->conf_state);
e501d055
AE		 652}
653
fc7f8a7e 654static void l2cap_do_start(struct l2cap_chan *chan)
79d554a6 655{
8c1d787b 656 struct l2cap_conn *conn = chan->conn;
79d554a6
MH		 657
658 if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) {
984947dc
MH		 659 if (!(conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE))
660 return;
661
d45fc423 662 if (l2cap_chan_check_security(chan) &&
4343478f 663 __l2cap_no_conn_pending(chan)) {
b1235d79 664 struct l2cap_conn_req req;
fe4128e0
GP		 665 req.scid = cpu_to_le16(chan->scid);
666 req.psm = chan->psm;
79d554a6 667
fc7f8a7e 668 chan->ident = l2cap_get_ident(conn);
c1360a1c 669 set_bit(CONF_CONNECT_PEND, &chan->conf_state);
79d554a6 670
fc7f8a7e
GP		 671 l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_REQ,
672 sizeof(req), &req);
b1235d79 673 }
79d554a6
MH		 674 } else {
675 struct l2cap_info_req req;
676 req.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
677
678 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
679 conn->info_ident = l2cap_get_ident(conn);
680
681 mod_timer(&conn->info_timer, jiffies +
682 msecs_to_jiffies(L2CAP_INFO_TIMEOUT));
683
684 l2cap_send_cmd(conn, conn->info_ident,
685 L2CAP_INFO_REQ, sizeof(req), &req);
686 }
687}
688
cf6c2c0b
GP		 689static inline int l2cap_mode_supported(__u8 mode, __u32 feat_mask)
690{
691 u32 local_feat_mask = l2cap_feat_mask;
d1c4a17d 692 if (!disable_ertm)
cf6c2c0b
GP		 693 local_feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING;
694
695 switch (mode) {
696 case L2CAP_MODE_ERTM:
697 return L2CAP_FEAT_ERTM & feat_mask & local_feat_mask;
698 case L2CAP_MODE_STREAMING:
699 return L2CAP_FEAT_STREAMING & feat_mask & local_feat_mask;
700 default:
701 return 0x00;
702 }
703}
704
4519de9a 705static void l2cap_send_disconn_req(struct l2cap_conn *conn, struct l2cap_chan *chan, int err)
22121fc9 706{
e92c8e70 707 struct sock *sk;
22121fc9
GP		 708 struct l2cap_disconn_req req;
709
c13ffa62
GP		 710 if (!conn)
711 return;
712
e92c8e70
GP		 713 sk = chan->sk;
714
0c1bc5c6 715 if (chan->mode == L2CAP_MODE_ERTM) {
1a09bcb9
GP		 716 __clear_retrans_timer(chan);
717 __clear_monitor_timer(chan);
718 __clear_ack_timer(chan);
c13ffa62
GP		 719 }
720
fe4128e0
GP		 721 req.dcid = cpu_to_le16(chan->dcid);
722 req.scid = cpu_to_le16(chan->scid);
22121fc9
GP		 723 l2cap_send_cmd(conn, l2cap_get_ident(conn),
724 L2CAP_DISCONN_REQ, sizeof(req), &req);
c13ffa62 725
89bc500e 726 l2cap_state_change(chan, BT_DISCONN);
9b108fc0 727 sk->sk_err = err;
22121fc9
GP		 728}
729
1da177e4 730/* ---- L2CAP connections ---- */
4e8402a3
MH		 731static void l2cap_conn_start(struct l2cap_conn *conn)
732{
820ffdb3 733 struct l2cap_chan *chan, *tmp;
4e8402a3
MH		 734
735 BT_DBG("conn %p", conn);
736
baa7e1fa 737 read_lock(&conn->chan_lock);
4e8402a3 738
820ffdb3 739 list_for_each_entry_safe(chan, tmp, &conn->chan_l, list) {
48454079 740 struct sock *sk = chan->sk;
baa7e1fa 741
4e8402a3
MH		 742 bh_lock_sock(sk);
743
715ec005 744 if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
79d554a6
MH		 745 bh_unlock_sock(sk);
746 continue;
747 }
748
89bc500e 749 if (chan->state == BT_CONNECT) {
47731de7 750 struct l2cap_conn_req req;
79d554a6 751
d45fc423 752 if (!l2cap_chan_check_security(chan) ||
b4450035 753 !__l2cap_no_conn_pending(chan)) {
47731de7
GP		 754 bh_unlock_sock(sk);
755 continue;
756 }
79d554a6 757
c1360a1c
GP		 758 if (!l2cap_mode_supported(chan->mode, conn->feat_mask)
759 && test_bit(CONF_STATE2_DEVICE,
760 &chan->conf_state)) {
0f852724 761 /* l2cap_chan_close() calls list_del(chan)
820ffdb3 762 * so release the lock */
2461daac 763 read_unlock(&conn->chan_lock);
89bc500e 764 l2cap_chan_close(chan, ECONNRESET);
2461daac 765 read_lock(&conn->chan_lock);
47731de7
GP		 766 bh_unlock_sock(sk);
767 continue;
b1235d79 768 }
47731de7 769
fe4128e0
GP		 770 req.scid = cpu_to_le16(chan->scid);
771 req.psm = chan->psm;
47731de7 772
fc7f8a7e 773 chan->ident = l2cap_get_ident(conn);
c1360a1c 774 set_bit(CONF_CONNECT_PEND, &chan->conf_state);
47731de7 775
fc7f8a7e
GP		 776 l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_REQ,
777 sizeof(req), &req);
47731de7 778
89bc500e 779 } else if (chan->state == BT_CONNECT2) {
79d554a6 780 struct l2cap_conn_rsp rsp;
e9aeb2dd 781 char buf[128];
fe4128e0
GP		 782 rsp.scid = cpu_to_le16(chan->dcid);
783 rsp.dcid = cpu_to_le16(chan->scid);
79d554a6 784
d45fc423 785 if (l2cap_chan_check_security(chan)) {
f66dc81f
MH		 786 if (bt_sk(sk)->defer_setup) {
787 struct sock *parent = bt_sk(sk)->parent;
788 rsp.result = cpu_to_le16(L2CAP_CR_PEND);
789 rsp.status = cpu_to_le16(L2CAP_CS_AUTHOR_PEND);
05e9a2f6
IK		 790 if (parent)
791 parent->sk_data_ready(parent, 0);
f66dc81f
MH		 792
793 } else {
89bc500e 794 l2cap_state_change(chan, BT_CONFIG);
f66dc81f
MH		 795 rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
796 rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
797 }
79d554a6
MH		 798 } else {
799 rsp.result = cpu_to_le16(L2CAP_CR_PEND);
800 rsp.status = cpu_to_le16(L2CAP_CS_AUTHEN_PEND);
801 }
802
fc7f8a7e
GP		 803 l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
804 sizeof(rsp), &rsp);
e9aeb2dd 805
c1360a1c 806 if (test_bit(CONF_REQ_SENT, &chan->conf_state) ||
e9aeb2dd
GP		 807 rsp.result != L2CAP_CR_SUCCESS) {
808 bh_unlock_sock(sk);
809 continue;
810 }
811
c1360a1c 812 set_bit(CONF_REQ_SENT, &chan->conf_state);
e9aeb2dd 813 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
73ffa904
GP		 814 l2cap_build_conf_req(chan, buf), buf);
815 chan->num_conf_req++;
4e8402a3
MH		 816 }
817
818 bh_unlock_sock(sk);
819 }
820
baa7e1fa 821 read_unlock(&conn->chan_lock);
4e8402a3
MH		 822}
823
b62f328b
VT		 824/* Find socket with cid and source bdaddr.
825 * Returns closest match, locked.
826 */
23691d75 827static struct l2cap_chan *l2cap_global_chan_by_scid(int state, __le16 cid, bdaddr_t *src)
b62f328b 828{
23691d75 829 struct l2cap_chan *c, *c1 = NULL;
b62f328b 830
23691d75 831 read_lock(&chan_list_lock);
b62f328b 832
23691d75
GP		 833 list_for_each_entry(c, &chan_list, global_l) {
834 struct sock *sk = c->sk;
fe4128e0 835
89bc500e 836 if (state && c->state != state)
b62f328b
VT		 837 continue;
838
23691d75 839 if (c->scid == cid) {
b62f328b 840 /* Exact match. */
23691d75
GP		 841 if (!bacmp(&bt_sk(sk)->src, src)) {
842 read_unlock(&chan_list_lock);
843 return c;
844 }
b62f328b
VT		 845
846 /* Closest match */
847 if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY))
23691d75 848 c1 = c;
b62f328b
VT		 849 }
850 }
280f294f 851
23691d75 852 read_unlock(&chan_list_lock);
b62f328b 853
23691d75 854 return c1;
b62f328b
VT		 855}
856
857static void l2cap_le_conn_ready(struct l2cap_conn *conn)
858{
c916fbe4 859 struct sock *parent, *sk;
23691d75 860 struct l2cap_chan *chan, *pchan;
b62f328b
VT		 861
862 BT_DBG("");
863
864 /* Check if we have socket listening on cid */
23691d75 865 pchan = l2cap_global_chan_by_scid(BT_LISTEN, L2CAP_CID_LE_DATA,
b62f328b 866 conn->src);
23691d75 867 if (!pchan)
b62f328b
VT		 868 return;
869
23691d75
GP		 870 parent = pchan->sk;
871
62f3a2cf
GP		 872 bh_lock_sock(parent);
873
b62f328b
VT		 874 /* Check for backlog size */
875 if (sk_acceptq_is_full(parent)) {
876 BT_DBG("backlog full %d", parent->sk_ack_backlog);
877 goto clean;
878 }
879
80808e43
GP		 880 chan = pchan->ops->new_connection(pchan->data);
881 if (!chan)
b62f328b
VT		 882 goto clean;
883
80808e43 884 sk = chan->sk;
5d41ce1d 885
baa7e1fa 886 write_lock_bh(&conn->chan_lock);
b62f328b
VT		 887
888 hci_conn_hold(conn->hcon);
889
b62f328b
VT		 890 bacpy(&bt_sk(sk)->src, conn->src);
891 bacpy(&bt_sk(sk)->dst, conn->dst);
892
d1010240
GP		 893 bt_accept_enqueue(parent, sk);
894
48454079
GP		 895 __l2cap_chan_add(conn, chan);
896
c9b66675 897 __set_chan_timer(chan, sk->sk_sndtimeo);
b62f328b 898
89bc500e 899 l2cap_state_change(chan, BT_CONNECTED);
b62f328b
VT		 900 parent->sk_data_ready(parent, 0);
901
baa7e1fa 902 write_unlock_bh(&conn->chan_lock);
b62f328b
VT		 903
904clean:
905 bh_unlock_sock(parent);
906}
907
f1cb9af5
VCG		 908static void l2cap_chan_ready(struct sock *sk)
909{
910 struct l2cap_chan *chan = l2cap_pi(sk)->chan;
911 struct sock *parent = bt_sk(sk)->parent;
912
913 BT_DBG("sk %p, parent %p", sk, parent);
914
915 chan->conf_state = 0;
916 __clear_chan_timer(chan);
917
43f3dc41 918 l2cap_state_change(chan, BT_CONNECTED);
f1cb9af5
VCG		 919 sk->sk_state_change(sk);
920
921 if (parent)
922 parent->sk_data_ready(parent, 0);
923}
924
4e8402a3
MH		 925static void l2cap_conn_ready(struct l2cap_conn *conn)
926{
48454079 927 struct l2cap_chan *chan;
4e8402a3 928
79d554a6 929 BT_DBG("conn %p", conn);
4e8402a3 930
b62f328b
VT		 931 if (!conn->hcon->out && conn->hcon->type == LE_LINK)
932 l2cap_le_conn_ready(conn);
933
160dc6ac
VCG		 934 if (conn->hcon->out && conn->hcon->type == LE_LINK)
935 smp_conn_security(conn, conn->hcon->pending_sec_level);
936
baa7e1fa 937 read_lock(&conn->chan_lock);
4e8402a3 938
baa7e1fa 939 list_for_each_entry(chan, &conn->chan_l, list) {
48454079 940 struct sock *sk = chan->sk;
baa7e1fa 941
79d554a6 942 bh_lock_sock(sk);
4e8402a3 943
63128451 944 if (conn->hcon->type == LE_LINK) {
b501d6a1 945 if (smp_conn_security(conn, chan->sec_level))
f1cb9af5 946 l2cap_chan_ready(sk);
acd7d370 947
63128451 948 } else if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
c9b66675 949 __clear_chan_timer(chan);
89bc500e 950 l2cap_state_change(chan, BT_CONNECTED);
79d554a6 951 sk->sk_state_change(sk);
b501d6a1 952
89bc500e 953 } else if (chan->state == BT_CONNECT)
fc7f8a7e 954 l2cap_do_start(chan);
4e8402a3 955
79d554a6 956 bh_unlock_sock(sk);
4e8402a3 957 }
79d554a6 958
baa7e1fa 959 read_unlock(&conn->chan_lock);
4e8402a3
MH		 960}
961
962/* Notify sockets that we cannot guaranty reliability anymore */
963static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err)
964{
48454079 965 struct l2cap_chan *chan;
4e8402a3
MH		 966
967 BT_DBG("conn %p", conn);
968
baa7e1fa 969 read_lock(&conn->chan_lock);
4e8402a3 970
baa7e1fa 971 list_for_each_entry(chan, &conn->chan_l, list) {
48454079 972 struct sock *sk = chan->sk;
baa7e1fa 973
ecf61bdb 974 if (test_bit(FLAG_FORCE_RELIABLE, &chan->flags))
4e8402a3
MH		 975 sk->sk_err = err;
976 }
977
baa7e1fa 978 read_unlock(&conn->chan_lock);
4e8402a3
MH		 979}
980
981static void l2cap_info_timeout(unsigned long arg)
982{
983 struct l2cap_conn *conn = (void *) arg;
984
984947dc 985 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
e1027a7c 986 conn->info_ident = 0;
984947dc 987
4e8402a3
MH		 988 l2cap_conn_start(conn);
989}
990
5d3de7df
VCG		 991static void l2cap_conn_del(struct hci_conn *hcon, int err)
992{
993 struct l2cap_conn *conn = hcon->l2cap_data;
994 struct l2cap_chan *chan, *l;
995 struct sock *sk;
996
997 if (!conn)
998 return;
999
1000 BT_DBG("hcon %p conn %p, err %d", hcon, conn, err);
1001
1002 kfree_skb(conn->rx_skb);
1003
1004 /* Kill channels */
1005 list_for_each_entry_safe(chan, l, &conn->chan_l, list) {
1006 sk = chan->sk;
1007 bh_lock_sock(sk);
1008 l2cap_chan_del(chan, err);
1009 bh_unlock_sock(sk);
1010 chan->ops->close(chan->data);
1011 }
1012
73d80deb
LAD		 1013 hci_chan_del(conn->hchan);
1014
5d3de7df
VCG		 1015 if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT)
1016 del_timer_sync(&conn->info_timer);
1017
d26a2345 1018 if (test_and_clear_bit(HCI_CONN_LE_SMP_PEND, &hcon->pend)) {
5d3de7df 1019 del_timer(&conn->security_timer);
8aab4757 1020 smp_chan_destroy(conn);
d26a2345 1021 }
5d3de7df
VCG		 1022
1023 hcon->l2cap_data = NULL;
1024 kfree(conn);
1025}
1026
1027static void security_timeout(unsigned long arg)
1028{
1029 struct l2cap_conn *conn = (void *) arg;
1030
1031 l2cap_conn_del(conn->hcon, ETIMEDOUT);
1032}
1033
1da177e4
LT		 1034static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status)
1035{
0139418c 1036 struct l2cap_conn *conn = hcon->l2cap_data;
73d80deb 1037 struct hci_chan *hchan;
1da177e4 1038
0139418c 1039 if (conn || status)
1da177e4
LT		 1040 return conn;
1041
73d80deb
LAD		 1042 hchan = hci_chan_create(hcon);
1043 if (!hchan)
1044 return NULL;
1045
0139418c 1046 conn = kzalloc(sizeof(struct l2cap_conn), GFP_ATOMIC);
73d80deb
LAD		 1047 if (!conn) {
1048 hci_chan_del(hchan);
1da177e4 1049 return NULL;
73d80deb 1050 }
1da177e4
LT		 1051
1052 hcon->l2cap_data = conn;
1053 conn->hcon = hcon;
73d80deb 1054 conn->hchan = hchan;
1da177e4 1055
73d80deb 1056 BT_DBG("hcon %p conn %p hchan %p", hcon, conn, hchan);
0139418c 1057
acd7d370
VT		 1058 if (hcon->hdev->le_mtu && hcon->type == LE_LINK)
1059 conn->mtu = hcon->hdev->le_mtu;
1060 else
1061 conn->mtu = hcon->hdev->acl_mtu;
1062
1da177e4
LT		 1063 conn->src = &hcon->hdev->bdaddr;
1064 conn->dst = &hcon->dst;
1065
4e8402a3
MH		 1066 conn->feat_mask = 0;
1067
1da177e4 1068 spin_lock_init(&conn->lock);
baa7e1fa
GP		 1069 rwlock_init(&conn->chan_lock);
1070
1071 INIT_LIST_HEAD(&conn->chan_l);
1da177e4 1072
5d3de7df
VCG		 1073 if (hcon->type == LE_LINK)
1074 setup_timer(&conn->security_timer, security_timeout,
1075 (unsigned long) conn);
1076 else
b62f328b 1077 setup_timer(&conn->info_timer, l2cap_info_timeout,
45054dc1
DY		 1078 (unsigned long) conn);
1079
9f5a0d7b 1080 conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM;
2950f21a 1081
1da177e4
LT		 1082 return conn;
1083}
1084
48454079 1085static inline void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
1da177e4 1086{
baa7e1fa 1087 write_lock_bh(&conn->chan_lock);
48454079 1088 __l2cap_chan_add(conn, chan);
baa7e1fa 1089 write_unlock_bh(&conn->chan_lock);
1da177e4
LT		 1090}
1091
1da177e4 1092/* ---- Socket interface ---- */
1da177e4
LT		 1093
1094/* Find socket with psm and source bdaddr.
1095 * Returns closest match.
1096 */
23691d75 1097static struct l2cap_chan *l2cap_global_chan_by_psm(int state, __le16 psm, bdaddr_t *src)
1da177e4 1098{
23691d75 1099 struct l2cap_chan *c, *c1 = NULL;
1da177e4 1100
23691d75 1101 read_lock(&chan_list_lock);
e0f0cb56 1102
23691d75
GP		 1103 list_for_each_entry(c, &chan_list, global_l) {
1104 struct sock *sk = c->sk;
fe4128e0 1105
89bc500e 1106 if (state && c->state != state)
1da177e4
LT		 1107 continue;
1108
23691d75 1109 if (c->psm == psm) {
1da177e4 1110 /* Exact match. */
23691d75 1111 if (!bacmp(&bt_sk(sk)->src, src)) {
a7567b20 1112 read_unlock(&chan_list_lock);
23691d75
GP		 1113 return c;
1114 }
1da177e4
LT		 1115
1116 /* Closest match */
1117 if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY))
23691d75 1118 c1 = c;
1da177e4
LT		 1119 }
1120 }
1da177e4 1121
23691d75 1122 read_unlock(&chan_list_lock);
e0f0cb56 1123
23691d75 1124 return c1;
1da177e4
LT		 1125}
1126
77a74c7e 1127int l2cap_chan_connect(struct l2cap_chan *chan)
1da177e4 1128{
5d41ce1d 1129 struct sock *sk = chan->sk;
1da177e4
LT		 1130 bdaddr_t *src = &bt_sk(sk)->src;
1131 bdaddr_t *dst = &bt_sk(sk)->dst;
1132 struct l2cap_conn *conn;
1133 struct hci_conn *hcon;
1134 struct hci_dev *hdev;
09ab6f4c 1135 __u8 auth_type;
44d0e48e 1136 int err;
1da177e4 1137
f29972de 1138 BT_DBG("%s -> %s psm 0x%2.2x", batostr(src), batostr(dst),
fe4128e0 1139 chan->psm);
1da177e4 1140
af05b30b
GP		 1141 hdev = hci_get_route(dst, src);
1142 if (!hdev)
1da177e4
LT		 1143 return -EHOSTUNREACH;
1144
1145 hci_dev_lock_bh(hdev);
1146
4343478f 1147 auth_type = l2cap_get_auth_type(chan);
09ab6f4c 1148
fe4128e0 1149 if (chan->dcid == L2CAP_CID_LE_DATA)
acd7d370 1150 hcon = hci_connect(hdev, LE_LINK, dst,
4343478f 1151 chan->sec_level, auth_type);
acd7d370
VT		 1152 else
1153 hcon = hci_connect(hdev, ACL_LINK, dst,
4343478f 1154 chan->sec_level, auth_type);
acd7d370 1155
30e76272
VT		 1156 if (IS_ERR(hcon)) {
1157 err = PTR_ERR(hcon);
1da177e4 1158 goto done;
30e76272 1159 }
1da177e4
LT		 1160
1161 conn = l2cap_conn_add(hcon, 0);
1162 if (!conn) {
1163 hci_conn_put(hcon);
30e76272 1164 err = -ENOMEM;
1da177e4
LT		 1165 goto done;
1166 }
1167
1da177e4
LT		 1168 /* Update source addr of the socket */
1169 bacpy(src, conn->src);
1170
48454079
GP		 1171 l2cap_chan_add(conn, chan);
1172
89bc500e 1173 l2cap_state_change(chan, BT_CONNECT);
c9b66675 1174 __set_chan_timer(chan, sk->sk_sndtimeo);
1da177e4
LT		 1175
1176 if (hcon->state == BT_CONNECTED) {
715ec005 1177 if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
c9b66675 1178 __clear_chan_timer(chan);
d45fc423 1179 if (l2cap_chan_check_security(chan))
89bc500e 1180 l2cap_state_change(chan, BT_CONNECTED);
79d554a6 1181 } else
fc7f8a7e 1182 l2cap_do_start(chan);
1da177e4
LT		 1183 }
1184
30e76272
VT		 1185 err = 0;
1186
1da177e4
LT		 1187done:
1188 hci_dev_unlock_bh(hdev);
1189 hci_dev_put(hdev);
1190 return err;
1191}
1192
dcba0dba 1193int __l2cap_wait_ack(struct sock *sk)
6161c038 1194{
8c1d787b 1195 struct l2cap_chan *chan = l2cap_pi(sk)->chan;
6161c038
GP		 1196 DECLARE_WAITQUEUE(wait, current);
1197 int err = 0;
1198 int timeo = HZ/5;
1199
2b0b05dd 1200 add_wait_queue(sk_sleep(sk), &wait);
a71a0cf4
PH		 1201 set_current_state(TASK_INTERRUPTIBLE);
1202 while (chan->unacked_frames > 0 && chan->conn) {
6161c038
GP		 1203 if (!timeo)
1204 timeo = HZ/5;
1205
1206 if (signal_pending(current)) {
1207 err = sock_intr_errno(timeo);
1208 break;
1209 }
1210
1211 release_sock(sk);
1212 timeo = schedule_timeout(timeo);
1213 lock_sock(sk);
a71a0cf4 1214 set_current_state(TASK_INTERRUPTIBLE);
6161c038
GP		 1215
1216 err = sock_error(sk);
1217 if (err)
1218 break;
1219 }
1220 set_current_state(TASK_RUNNING);
2b0b05dd 1221 remove_wait_queue(sk_sleep(sk), &wait);
6161c038
GP		 1222 return err;
1223}
1224
e90bac06
GP		 1225static void l2cap_monitor_timeout(unsigned long arg)
1226{
525cd185
GP		 1227 struct l2cap_chan *chan = (void *) arg;
1228 struct sock *sk = chan->sk;
e90bac06 1229
525cd185 1230 BT_DBG("chan %p", chan);
0e98958d 1231
e686219a 1232 bh_lock_sock(sk);
2c03a7a4 1233 if (chan->retry_count >= chan->remote_max_tx) {
8c1d787b 1234 l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED);
b13f5860 1235 bh_unlock_sock(sk);
e90bac06
GP		 1236 return;
1237 }
1238
6a026610 1239 chan->retry_count++;
1a09bcb9 1240 __set_monitor_timer(chan);
e90bac06 1241
525cd185 1242 l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_POLL);
e686219a 1243 bh_unlock_sock(sk);
e90bac06
GP		 1244}
1245
1246static void l2cap_retrans_timeout(unsigned long arg)
1247{
525cd185
GP		 1248 struct l2cap_chan *chan = (void *) arg;
1249 struct sock *sk = chan->sk;
e90bac06 1250
49208c9c 1251 BT_DBG("chan %p", chan);
0e98958d 1252
e686219a 1253 bh_lock_sock(sk);
6a026610 1254 chan->retry_count = 1;
1a09bcb9 1255 __set_monitor_timer(chan);
e90bac06 1256
e2ab4353 1257 set_bit(CONN_WAIT_F, &chan->conn_state);
e90bac06 1258
525cd185 1259 l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_POLL);
e686219a 1260 bh_unlock_sock(sk);
e90bac06
GP		 1261}
1262
42e5c802 1263static void l2cap_drop_acked_frames(struct l2cap_chan *chan)
1da177e4 1264{
1c2acffb 1265 struct sk_buff *skb;
1da177e4 1266
58d35f87 1267 while ((skb = skb_peek(&chan->tx_q)) &&
6a026610 1268 chan->unacked_frames) {
42e5c802 1269 if (bt_cb(skb)->tx_seq == chan->expected_ack_seq)
1c2acffb 1270 break;
1da177e4 1271
58d35f87 1272 skb = skb_dequeue(&chan->tx_q);
1c2acffb 1273 kfree_skb(skb);
1da177e4 1274
6a026610 1275 chan->unacked_frames--;
1c2acffb 1276 }
1da177e4 1277
6a026610 1278 if (!chan->unacked_frames)
1a09bcb9 1279 __clear_retrans_timer(chan);
1c2acffb 1280}
1da177e4 1281
67c9e840 1282static void l2cap_streaming_send(struct l2cap_chan *chan)
6840ed07 1283{
ccbb84af 1284 struct sk_buff *skb;
88843ab0
AE		 1285 u32 control;
1286 u16 fcs;
6840ed07 1287
58d35f87 1288 while ((skb = skb_dequeue(&chan->tx_q))) {
88843ab0 1289 control = __get_control(chan, skb->data + L2CAP_HDR_SIZE);
fb45de7d 1290 control |= __set_txseq(chan, chan->next_tx_seq);
88843ab0 1291 __put_control(chan, control, skb->data + L2CAP_HDR_SIZE);
6840ed07 1292
47d1ec61 1293 if (chan->fcs == L2CAP_FCS_CRC16) {
03a51213
AE		 1294 fcs = crc16(0, (u8 *)skb->data,
1295 skb->len - L2CAP_FCS_SIZE);
1296 put_unaligned_le16(fcs,
1297 skb->data + skb->len - L2CAP_FCS_SIZE);
fcc203c3
GP		 1298 }
1299
4343478f 1300 l2cap_do_send(chan, skb);
6840ed07 1301
836be934 1302 chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq);
6840ed07 1303 }
6840ed07
GP		 1304}
1305
fb45de7d 1306static void l2cap_retransmit_one_frame(struct l2cap_chan *chan, u16 tx_seq)
8f17154f 1307{
8f17154f 1308 struct sk_buff *skb, *tx_skb;
88843ab0
AE		 1309 u16 fcs;
1310 u32 control;
8f17154f 1311
58d35f87 1312 skb = skb_peek(&chan->tx_q);
f11d676d
GP		 1313 if (!skb)
1314 return;
8f17154f 1315
d1726b6d 1316 while (bt_cb(skb)->tx_seq != tx_seq) {
58d35f87 1317 if (skb_queue_is_last(&chan->tx_q, skb))
f11d676d 1318 return;
8f17154f 1319
d1726b6d
SJ		 1320 skb = skb_queue_next(&chan->tx_q, skb);
1321 }
8f17154f 1322
2c03a7a4
GP		 1323 if (chan->remote_max_tx &&
1324 bt_cb(skb)->retries == chan->remote_max_tx) {
8c1d787b 1325 l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED);
f11d676d
GP		 1326 return;
1327 }
1328
1329 tx_skb = skb_clone(skb, GFP_ATOMIC);
1330 bt_cb(skb)->retries++;
88843ab0
AE		 1331
1332 control = __get_control(chan, tx_skb->data + L2CAP_HDR_SIZE);
7e0ef6ee 1333 control &= __get_sar_mask(chan);
3cb123d1 1334
e2ab4353 1335 if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
03f6715d 1336 control |= __set_ctrl_final(chan);
95ffa978 1337
0b209fae 1338 control |= __set_reqseq(chan, chan->buffer_seq);
fb45de7d 1339 control |= __set_txseq(chan, tx_seq);
3cb123d1 1340
88843ab0 1341 __put_control(chan, control, tx_skb->data + L2CAP_HDR_SIZE);
f11d676d 1342
47d1ec61 1343 if (chan->fcs == L2CAP_FCS_CRC16) {
03a51213
AE		 1344 fcs = crc16(0, (u8 *)tx_skb->data,
1345 tx_skb->len - L2CAP_FCS_SIZE);
1346 put_unaligned_le16(fcs,
1347 tx_skb->data + tx_skb->len - L2CAP_FCS_SIZE);
f11d676d
GP		 1348 }
1349
4343478f 1350 l2cap_do_send(chan, tx_skb);
8f17154f
GP		 1351}
1352
67c9e840 1353static int l2cap_ertm_send(struct l2cap_chan *chan)
1c2acffb
GP		 1354{
1355 struct sk_buff *skb, *tx_skb;
88843ab0
AE		 1356 u16 fcs;
1357 u32 control;
9a9c6a34 1358 int nsent = 0;
1c2acffb 1359
89bc500e 1360 if (chan->state != BT_CONNECTED)
c13ffa62 1361 return -ENOTCONN;
e90bac06 1362
58d35f87 1363 while ((skb = chan->tx_send_head) && (!l2cap_tx_window_full(chan))) {
1c2acffb 1364
2c03a7a4
GP		 1365 if (chan->remote_max_tx &&
1366 bt_cb(skb)->retries == chan->remote_max_tx) {
8c1d787b 1367 l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED);
e90bac06
GP		 1368 break;
1369 }
1370
e420aba3
AE		 1371 tx_skb = skb_clone(skb, GFP_ATOMIC);
1372
e90bac06
GP		 1373 bt_cb(skb)->retries++;
1374
88843ab0 1375 control = __get_control(chan, tx_skb->data + L2CAP_HDR_SIZE);
7e0ef6ee 1376 control &= __get_sar_mask(chan);
95ffa978 1377
e2ab4353 1378 if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
03f6715d 1379 control |= __set_ctrl_final(chan);
e2ab4353 1380
0b209fae 1381 control |= __set_reqseq(chan, chan->buffer_seq);
fb45de7d 1382 control |= __set_txseq(chan, chan->next_tx_seq);
1c2acffb 1383
88843ab0 1384 __put_control(chan, control, tx_skb->data + L2CAP_HDR_SIZE);
e90bac06 1385
47d1ec61 1386 if (chan->fcs == L2CAP_FCS_CRC16) {
03a51213
AE		 1387 fcs = crc16(0, (u8 *)skb->data,
1388 tx_skb->len - L2CAP_FCS_SIZE);
1389 put_unaligned_le16(fcs, skb->data +
1390 tx_skb->len - L2CAP_FCS_SIZE);
fcc203c3
GP		 1391 }
1392
4343478f 1393 l2cap_do_send(chan, tx_skb);
9a9c6a34 1394
1a09bcb9 1395 __set_retrans_timer(chan);
1c2acffb 1396
42e5c802 1397 bt_cb(skb)->tx_seq = chan->next_tx_seq;
836be934
AE		 1398
1399 chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq);
1c2acffb 1400
23e9fde2 1401 if (bt_cb(skb)->retries == 1)
6a026610 1402 chan->unacked_frames++;
23e9fde2 1403
6a026610 1404 chan->frames_sent++;
1c2acffb 1405
58d35f87
GP		 1406 if (skb_queue_is_last(&chan->tx_q, skb))
1407 chan->tx_send_head = NULL;
1c2acffb 1408 else
58d35f87 1409 chan->tx_send_head = skb_queue_next(&chan->tx_q, skb);
9e917af1
GP		 1410
1411 nsent++;
1c2acffb
GP		 1412 }
1413
9e917af1
GP		 1414 return nsent;
1415}
1416
525cd185 1417static int l2cap_retransmit_frames(struct l2cap_chan *chan)
dfc909be 1418{
dfc909be
GP		 1419 int ret;
1420
58d35f87
GP		 1421 if (!skb_queue_empty(&chan->tx_q))
1422 chan->tx_send_head = chan->tx_q.next;
dfc909be 1423
42e5c802 1424 chan->next_tx_seq = chan->expected_ack_seq;
525cd185 1425 ret = l2cap_ertm_send(chan);
dfc909be
GP		 1426 return ret;
1427}
1428
525cd185 1429static void l2cap_send_ack(struct l2cap_chan *chan)
9e917af1 1430{
88843ab0 1431 u32 control = 0;
9e917af1 1432
0b209fae 1433 control |= __set_reqseq(chan, chan->buffer_seq);
9e917af1 1434
e2ab4353 1435 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
ab784b73 1436 control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR);
e2ab4353 1437 set_bit(CONN_RNR_SENT, &chan->conn_state);
525cd185 1438 l2cap_send_sframe(chan, control);
9a9c6a34 1439 return;
9e917af1 1440 }
dfc909be 1441
525cd185 1442 if (l2cap_ertm_send(chan) > 0)
dfc909be
GP		 1443 return;
1444
ab784b73 1445 control |= __set_ctrl_super(chan, L2CAP_SUPER_RR);
525cd185 1446 l2cap_send_sframe(chan, control);
1c2acffb
GP		 1447}
1448
525cd185 1449static void l2cap_send_srejtail(struct l2cap_chan *chan)
99b0d4b7
GP		 1450{
1451 struct srej_list *tail;
88843ab0 1452 u32 control;
99b0d4b7 1453
ab784b73 1454 control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ);
03f6715d 1455 control |= __set_ctrl_final(chan);
99b0d4b7 1456
39d5a3ee 1457 tail = list_entry((&chan->srej_l)->prev, struct srej_list, list);
0b209fae 1458 control |= __set_reqseq(chan, tail->tx_seq);
99b0d4b7 1459
525cd185 1460 l2cap_send_sframe(chan, control);
99b0d4b7
GP		 1461}
1462
1c2acffb
GP		 1463static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, int len, int count, struct sk_buff *skb)
1464{
8c1d787b 1465 struct l2cap_conn *conn = l2cap_pi(sk)->chan->conn;
1c2acffb
GP		 1466 struct sk_buff **frag;
1467 int err, sent = 0;
1da177e4 1468
59203a21 1469 if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count))
1c2acffb 1470 return -EFAULT;
1da177e4
LT		 1471
1472 sent += count;
1473 len -= count;
1474
1475 /* Continuation fragments (no L2CAP header) */
1476 frag = &skb_shinfo(skb)->frag_list;
1477 while (len) {
1478 count = min_t(unsigned int, conn->mtu, len);
1479
1480 *frag = bt_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err);
1481 if (!*frag)
0175d629 1482 return err;
1c2acffb
GP		 1483 if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count))
1484 return -EFAULT;
1da177e4 1485
5e59b791
LAD		 1486 (*frag)->priority = skb->priority;
1487
1da177e4
LT		 1488 sent += count;
1489 len -= count;
1490
1491 frag = &(*frag)->next;
1492 }
1da177e4
LT		 1493
1494 return sent;
1c2acffb 1495}
1da177e4 1496
5e59b791
LAD		 1497static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan,
1498 struct msghdr *msg, size_t len,
1499 u32 priority)
1c2acffb 1500{
fe4128e0 1501 struct sock *sk = chan->sk;
8c1d787b 1502 struct l2cap_conn *conn = chan->conn;
1c2acffb 1503 struct sk_buff *skb;
03a51213 1504 int err, count, hlen = L2CAP_HDR_SIZE + L2CAP_PSMLEN_SIZE;
1c2acffb
GP		 1505 struct l2cap_hdr *lh;
1506
5e59b791 1507 BT_DBG("sk %p len %d priority %u", sk, (int)len, priority);
1c2acffb
GP		 1508
1509 count = min_t(unsigned int, (conn->mtu - hlen), len);
1510 skb = bt_skb_send_alloc(sk, count + hlen,
1511 msg->msg_flags & MSG_DONTWAIT, &err);
1512 if (!skb)
0175d629 1513 return ERR_PTR(err);
1c2acffb 1514
5e59b791
LAD		 1515 skb->priority = priority;
1516
1c2acffb
GP		 1517 /* Create L2CAP header */
1518 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
fe4128e0 1519 lh->cid = cpu_to_le16(chan->dcid);
1c2acffb 1520 lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
fe4128e0 1521 put_unaligned_le16(chan->psm, skb_put(skb, 2));
1c2acffb
GP		 1522
1523 err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1524 if (unlikely(err < 0)) {
1525 kfree_skb(skb);
1526 return ERR_PTR(err);
1527 }
1528 return skb;
1529}
1530
5e59b791
LAD		 1531static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan,
1532 struct msghdr *msg, size_t len,
1533 u32 priority)
1c2acffb 1534{
fe4128e0 1535 struct sock *sk = chan->sk;
8c1d787b 1536 struct l2cap_conn *conn = chan->conn;
1c2acffb
GP		 1537 struct sk_buff *skb;
1538 int err, count, hlen = L2CAP_HDR_SIZE;
1539 struct l2cap_hdr *lh;
1540
1541 BT_DBG("sk %p len %d", sk, (int)len);
1542
1543 count = min_t(unsigned int, (conn->mtu - hlen), len);
1544 skb = bt_skb_send_alloc(sk, count + hlen,
1545 msg->msg_flags & MSG_DONTWAIT, &err);
1546 if (!skb)
0175d629 1547 return ERR_PTR(err);
1c2acffb 1548
5e59b791
LAD		 1549 skb->priority = priority;
1550
1c2acffb
GP		 1551 /* Create L2CAP header */
1552 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
fe4128e0 1553 lh->cid = cpu_to_le16(chan->dcid);
1c2acffb
GP		 1554 lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1555
1556 err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1557 if (unlikely(err < 0)) {
1558 kfree_skb(skb);
1559 return ERR_PTR(err);
1560 }
1561 return skb;
1562}
1563
ab0ff76d
LAD		 1564static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan,
1565 struct msghdr *msg, size_t len,
88843ab0 1566 u32 control, u16 sdulen)
1c2acffb 1567{
47d1ec61 1568 struct sock *sk = chan->sk;
8c1d787b 1569 struct l2cap_conn *conn = chan->conn;
1c2acffb 1570 struct sk_buff *skb;
e4ca6d98 1571 int err, count, hlen;
1c2acffb
GP		 1572 struct l2cap_hdr *lh;
1573
1574 BT_DBG("sk %p len %d", sk, (int)len);
1575
0ee0d208
GP		 1576 if (!conn)
1577 return ERR_PTR(-ENOTCONN);
1578
e4ca6d98
AE		 1579 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
1580 hlen = L2CAP_EXT_HDR_SIZE;
1581 else
1582 hlen = L2CAP_ENH_HDR_SIZE;
1583
c74e560c 1584 if (sdulen)
03a51213 1585 hlen += L2CAP_SDULEN_SIZE;
c74e560c 1586
47d1ec61 1587 if (chan->fcs == L2CAP_FCS_CRC16)
03a51213 1588 hlen += L2CAP_FCS_SIZE;
fcc203c3 1589
1c2acffb
GP		 1590 count = min_t(unsigned int, (conn->mtu - hlen), len);
1591 skb = bt_skb_send_alloc(sk, count + hlen,
1592 msg->msg_flags & MSG_DONTWAIT, &err);
1593 if (!skb)
0175d629 1594 return ERR_PTR(err);
1c2acffb
GP		 1595
1596 /* Create L2CAP header */
1597 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
fe4128e0 1598 lh->cid = cpu_to_le16(chan->dcid);
1c2acffb 1599 lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
88843ab0
AE		 1600
1601 __put_control(chan, control, skb_put(skb, __ctrl_size(chan)));
1602
c74e560c 1603 if (sdulen)
03a51213 1604 put_unaligned_le16(sdulen, skb_put(skb, L2CAP_SDULEN_SIZE));
1c2acffb
GP		 1605
1606 err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1607 if (unlikely(err < 0)) {
1608 kfree_skb(skb);
1609 return ERR_PTR(err);
1610 }
e90bac06 1611
47d1ec61 1612 if (chan->fcs == L2CAP_FCS_CRC16)
03a51213 1613 put_unaligned_le16(0, skb_put(skb, L2CAP_FCS_SIZE));
fcc203c3 1614
e90bac06 1615 bt_cb(skb)->retries = 0;
1c2acffb 1616 return skb;
1da177e4
LT		 1617}
1618
67c9e840 1619static int l2cap_sar_segment_sdu(struct l2cap_chan *chan, struct msghdr *msg, size_t len)
c74e560c 1620{
c74e560c
GP		 1621 struct sk_buff *skb;
1622 struct sk_buff_head sar_queue;
88843ab0 1623 u32 control;
c74e560c
GP		 1624 size_t size = 0;
1625
ff12fd64 1626 skb_queue_head_init(&sar_queue);
7e0ef6ee 1627 control = __set_ctrl_sar(chan, L2CAP_SAR_START);
47d1ec61 1628 skb = l2cap_create_iframe_pdu(chan, msg, chan->remote_mps, control, len);
c74e560c
GP		 1629 if (IS_ERR(skb))
1630 return PTR_ERR(skb);
1631
1632 __skb_queue_tail(&sar_queue, skb);
2c03a7a4
GP		 1633 len -= chan->remote_mps;
1634 size += chan->remote_mps;
c74e560c
GP		 1635
1636 while (len > 0) {
1637 size_t buflen;
1638
2c03a7a4 1639 if (len > chan->remote_mps) {
7e0ef6ee 1640 control = __set_ctrl_sar(chan, L2CAP_SAR_CONTINUE);
2c03a7a4 1641 buflen = chan->remote_mps;
c74e560c 1642 } else {
7e0ef6ee 1643 control = __set_ctrl_sar(chan, L2CAP_SAR_END);
c74e560c
GP		 1644 buflen = len;
1645 }
1646
47d1ec61 1647 skb = l2cap_create_iframe_pdu(chan, msg, buflen, control, 0);
c74e560c
GP		 1648 if (IS_ERR(skb)) {
1649 skb_queue_purge(&sar_queue);
1650 return PTR_ERR(skb);
1651 }
1652
1653 __skb_queue_tail(&sar_queue, skb);
1654 len -= buflen;
1655 size += buflen;
c74e560c 1656 }
58d35f87
GP		 1657 skb_queue_splice_tail(&sar_queue, &chan->tx_q);
1658 if (chan->tx_send_head == NULL)
1659 chan->tx_send_head = sar_queue.next;
c74e560c
GP		 1660
1661 return size;
1662}
1663
5e59b791
LAD		 1664int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len,
1665 u32 priority)
9a91a04a 1666{
9a91a04a 1667 struct sk_buff *skb;
88843ab0 1668 u32 control;
9a91a04a
GP		 1669 int err;
1670
1671 /* Connectionless channel */
715ec005 1672 if (chan->chan_type == L2CAP_CHAN_CONN_LESS) {
5e59b791 1673 skb = l2cap_create_connless_pdu(chan, msg, len, priority);
9a91a04a
GP		 1674 if (IS_ERR(skb))
1675 return PTR_ERR(skb);
1676
1677 l2cap_do_send(chan, skb);
1678 return len;
1679 }
1680
1681 switch (chan->mode) {
1682 case L2CAP_MODE_BASIC:
1683 /* Check outgoing MTU */
1684 if (len > chan->omtu)
1685 return -EMSGSIZE;
1686
1687 /* Create a basic PDU */
5e59b791 1688 skb = l2cap_create_basic_pdu(chan, msg, len, priority);
9a91a04a
GP		 1689 if (IS_ERR(skb))
1690 return PTR_ERR(skb);
1691
1692 l2cap_do_send(chan, skb);
1693 err = len;
1694 break;
1695
1696 case L2CAP_MODE_ERTM:
1697 case L2CAP_MODE_STREAMING:
1698 /* Entire SDU fits into one PDU */
1699 if (len <= chan->remote_mps) {
7e0ef6ee 1700 control = __set_ctrl_sar(chan, L2CAP_SAR_UNSEGMENTED);
9a91a04a
GP		 1701 skb = l2cap_create_iframe_pdu(chan, msg, len, control,
1702 0);
1703 if (IS_ERR(skb))
1704 return PTR_ERR(skb);
1705
1706 __skb_queue_tail(&chan->tx_q, skb);
1707
1708 if (chan->tx_send_head == NULL)
1709 chan->tx_send_head = skb;
1710
1711 } else {
1712 /* Segment SDU into multiples PDUs */
1713 err = l2cap_sar_segment_sdu(chan, msg, len);
1714 if (err < 0)
1715 return err;
1716 }
1717
1718 if (chan->mode == L2CAP_MODE_STREAMING) {
1719 l2cap_streaming_send(chan);
1720 err = len;
1721 break;
1722 }
1723
e2ab4353
GP		 1724 if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
1725 test_bit(CONN_WAIT_F, &chan->conn_state)) {
9a91a04a
GP		 1726 err = len;
1727 break;
1728 }
1729
1730 err = l2cap_ertm_send(chan);
1731 if (err >= 0)
1732 err = len;
1733
1734 break;
1735
1736 default:
1737 BT_DBG("bad state %1.1x", chan->mode);
1738 err = -EBADFD;
1739 }
1740
1741 return err;
1742}
1743
1da177e4
LT		 1744/* Copy frame to all raw sockets on that connection */
1745static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb)
1746{
1da177e4 1747 struct sk_buff *nskb;
48454079 1748 struct l2cap_chan *chan;
1da177e4
LT		 1749
1750 BT_DBG("conn %p", conn);
1751
baa7e1fa
GP		 1752 read_lock(&conn->chan_lock);
1753 list_for_each_entry(chan, &conn->chan_l, list) {
48454079 1754 struct sock *sk = chan->sk;
715ec005 1755 if (chan->chan_type != L2CAP_CHAN_RAW)
1da177e4
LT		 1756 continue;
1757
1758 /* Don't send frame to the socket it came from */
1759 if (skb->sk == sk)
1760 continue;
af05b30b
GP		 1761 nskb = skb_clone(skb, GFP_ATOMIC);
1762 if (!nskb)
1da177e4
LT		 1763 continue;
1764
23070494 1765 if (chan->ops->recv(chan->data, nskb))
1da177e4
LT		 1766 kfree_skb(nskb);
1767 }
baa7e1fa 1768 read_unlock(&conn->chan_lock);
1da177e4
LT		 1769}
1770
1771/* ---- L2CAP signalling commands ---- */
1772static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
1773 u8 code, u8 ident, u16 dlen, void *data)
1774{
1775 struct sk_buff *skb, **frag;
1776 struct l2cap_cmd_hdr *cmd;
1777 struct l2cap_hdr *lh;
1778 int len, count;
1779
af05b30b
GP		 1780 BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %d",
1781 conn, code, ident, dlen);
1da177e4
LT		 1782
1783 len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen;
1784 count = min_t(unsigned int, conn->mtu, len);
1785
1786 skb = bt_skb_alloc(count, GFP_ATOMIC);
1787 if (!skb)
1788 return NULL;
1789
1790 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
aca3192c 1791 lh->len = cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen);
3300d9a9
CT		 1792
1793 if (conn->hcon->type == LE_LINK)
1794 lh->cid = cpu_to_le16(L2CAP_CID_LE_SIGNALING);
1795 else
1796 lh->cid = cpu_to_le16(L2CAP_CID_SIGNALING);
1da177e4
LT		 1797
1798 cmd = (struct l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE);
1799 cmd->code = code;
1800 cmd->ident = ident;
aca3192c 1801 cmd->len = cpu_to_le16(dlen);
1da177e4
LT		 1802
1803 if (dlen) {
1804 count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE;
1805 memcpy(skb_put(skb, count), data, count);
1806 data += count;
1807 }
1808
1809 len -= skb->len;
1810
1811 /* Continuation fragments (no L2CAP header) */
1812 frag = &skb_shinfo(skb)->frag_list;
1813 while (len) {
1814 count = min_t(unsigned int, conn->mtu, len);
1815
1816 *frag = bt_skb_alloc(count, GFP_ATOMIC);
1817 if (!*frag)
1818 goto fail;
1819
1820 memcpy(skb_put(*frag, count), data, count);
1821
1822 len -= count;
1823 data += count;
1824
1825 frag = &(*frag)->next;
1826 }
1827
1828 return skb;
1829
1830fail:
1831 kfree_skb(skb);
1832 return NULL;
1833}
1834
1835static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, unsigned long *val)
1836{
1837 struct l2cap_conf_opt *opt = *ptr;
1838 int len;
1839
1840 len = L2CAP_CONF_OPT_SIZE + opt->len;
1841 *ptr += len;
1842
1843 *type = opt->type;
1844 *olen = opt->len;
1845
1846 switch (opt->len) {
1847 case 1:
1848 *val = *((u8 *) opt->val);
1849 break;
1850
1851 case 2:
bfaaeb3e 1852 *val = get_unaligned_le16(opt->val);
1da177e4
LT		 1853 break;
1854
1855 case 4:
bfaaeb3e 1856 *val = get_unaligned_le32(opt->val);
1da177e4
LT		 1857 break;
1858
1859 default:
1860 *val = (unsigned long) opt->val;
1861 break;
1862 }
1863
1864 BT_DBG("type 0x%2.2x len %d val 0x%lx", *type, opt->len, *val);
1865 return len;
1866}
1867
1da177e4
LT		 1868static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val)
1869{
1870 struct l2cap_conf_opt *opt = *ptr;
1871
1872 BT_DBG("type 0x%2.2x len %d val 0x%lx", type, len, val);
1873
1874 opt->type = type;
1875 opt->len = len;
1876
1877 switch (len) {
1878 case 1:
1879 *((u8 *) opt->val) = val;
1880 break;
1881
1882 case 2:
4f8b691c 1883 put_unaligned_le16(val, opt->val);
1da177e4
LT		 1884 break;
1885
1886 case 4:
4f8b691c 1887 put_unaligned_le32(val, opt->val);
1da177e4
LT		 1888 break;
1889
1890 default:
1891 memcpy(opt->val, (void *) val, len);
1892 break;
1893 }
1894
1895 *ptr += L2CAP_CONF_OPT_SIZE + len;
1896}
1897
f89cef09
AE		 1898static void l2cap_add_opt_efs(void **ptr, struct l2cap_chan *chan)
1899{
1900 struct l2cap_conf_efs efs;
1901
1ec918ce 1902 switch (chan->mode) {
f89cef09
AE		 1903 case L2CAP_MODE_ERTM:
1904 efs.id = chan->local_id;
1905 efs.stype = chan->local_stype;
1906 efs.msdu = cpu_to_le16(chan->local_msdu);
1907 efs.sdu_itime = cpu_to_le32(chan->local_sdu_itime);
1908 efs.acc_lat = cpu_to_le32(L2CAP_DEFAULT_ACC_LAT);
1909 efs.flush_to = cpu_to_le32(L2CAP_DEFAULT_FLUSH_TO);
1910 break;
1911
1912 case L2CAP_MODE_STREAMING:
1913 efs.id = 1;
1914 efs.stype = L2CAP_SERV_BESTEFFORT;
1915 efs.msdu = cpu_to_le16(chan->local_msdu);
1916 efs.sdu_itime = cpu_to_le32(chan->local_sdu_itime);
1917 efs.acc_lat = 0;
1918 efs.flush_to = 0;
1919 break;
1920
1921 default:
1922 return;
1923 }
1924
1925 l2cap_add_conf_opt(ptr, L2CAP_CONF_EFS, sizeof(efs),
1926 (unsigned long) &efs);
1927}
1928
c1b4f43b
GP		 1929static void l2cap_ack_timeout(unsigned long arg)
1930{
525cd185 1931 struct l2cap_chan *chan = (void *) arg;
c1b4f43b 1932
525cd185
GP		 1933 bh_lock_sock(chan->sk);
1934 l2cap_send_ack(chan);
1935 bh_unlock_sock(chan->sk);
c1b4f43b
GP		 1936}
1937
525cd185 1938static inline void l2cap_ertm_init(struct l2cap_chan *chan)
0565c1c2 1939{
525cd185
GP		 1940 struct sock *sk = chan->sk;
1941
42e5c802 1942 chan->expected_ack_seq = 0;
6a026610 1943 chan->unacked_frames = 0;
42e5c802 1944 chan->buffer_seq = 0;
6a026610
GP		 1945 chan->num_acked = 0;
1946 chan->frames_sent = 0;
0565c1c2 1947
e92c8e70
GP		 1948 setup_timer(&chan->retrans_timer, l2cap_retrans_timeout,
1949 (unsigned long) chan);
1950 setup_timer(&chan->monitor_timer, l2cap_monitor_timeout,
1951 (unsigned long) chan);
1952 setup_timer(&chan->ack_timer, l2cap_ack_timeout, (unsigned long) chan);
0565c1c2 1953
f1c6775b 1954 skb_queue_head_init(&chan->srej_q);
1890d36b 1955
39d5a3ee
GP		 1956 INIT_LIST_HEAD(&chan->srej_l);
1957
218bb9df
GP		 1958
1959 sk->sk_backlog_rcv = l2cap_ertm_data_rcv;
0565c1c2
GP		 1960}
1961
f2fcfcd6
GP		 1962static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask)
1963{
1964 switch (mode) {
1965 case L2CAP_MODE_STREAMING:
1966 case L2CAP_MODE_ERTM:
1967 if (l2cap_mode_supported(mode, remote_feat_mask))
1968 return mode;
1969 /* fall through */
1970 default:
1971 return L2CAP_MODE_BASIC;
1972 }
1973}
1974
6327eb98
AE		 1975static inline bool __l2cap_ews_supported(struct l2cap_chan *chan)
1976{
1977 return enable_hs && chan->conn->feat_mask & L2CAP_FEAT_EXT_WINDOW;
1978}
1979
f89cef09
AE		 1980static inline bool __l2cap_efs_supported(struct l2cap_chan *chan)
1981{
1982 return enable_hs && chan->conn->feat_mask & L2CAP_FEAT_EXT_FLOW;
1983}
1984
6327eb98
AE		 1985static inline void l2cap_txwin_setup(struct l2cap_chan *chan)
1986{
1987 if (chan->tx_win > L2CAP_DEFAULT_TX_WINDOW &&
836be934 1988 __l2cap_ews_supported(chan)) {
6327eb98
AE		 1989 /* use extended control field */
1990 set_bit(FLAG_EXT_CTRL, &chan->flags);
836be934
AE		 1991 chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
1992 } else {
6327eb98
AE		 1993 chan->tx_win = min_t(u16, chan->tx_win,
1994 L2CAP_DEFAULT_TX_WINDOW);
836be934
AE		 1995 chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW;
1996 }
6327eb98
AE		 1997}
1998
710f9b0a 1999static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data)
1da177e4 2000{
1da177e4 2001 struct l2cap_conf_req *req = data;
0c1bc5c6 2002 struct l2cap_conf_rfc rfc = { .mode = chan->mode };
1da177e4 2003 void *ptr = req->data;
c8f79162 2004 u16 size;
1da177e4 2005
49208c9c 2006 BT_DBG("chan %p", chan);
1da177e4 2007
73ffa904 2008 if (chan->num_conf_req || chan->num_conf_rsp)
f2fcfcd6
GP		 2009 goto done;
2010
0c1bc5c6 2011 switch (chan->mode) {
f2fcfcd6
GP		 2012 case L2CAP_MODE_STREAMING:
2013 case L2CAP_MODE_ERTM:
c1360a1c 2014 if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state))
85eb53c6 2015 break;
85eb53c6 2016
f89cef09
AE		 2017 if (__l2cap_efs_supported(chan))
2018 set_bit(FLAG_EFS_ENABLE, &chan->flags);
2019
2ba13ed6 2020 /* fall through */
f2fcfcd6 2021 default:
8c1d787b 2022 chan->mode = l2cap_select_mode(rfc.mode, chan->conn->feat_mask);
f2fcfcd6
GP		 2023 break;
2024 }
2025
2026done:
0c1bc5c6
GP		 2027 if (chan->imtu != L2CAP_DEFAULT_MTU)
2028 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu);
7990681c 2029
0c1bc5c6 2030 switch (chan->mode) {
65c7c491 2031 case L2CAP_MODE_BASIC:
8c1d787b
GP		 2032 if (!(chan->conn->feat_mask & L2CAP_FEAT_ERTM) &&
2033 !(chan->conn->feat_mask & L2CAP_FEAT_STREAMING))
63406504
GP		 2034 break;
2035
62547752
GP		 2036 rfc.mode = L2CAP_MODE_BASIC;
2037 rfc.txwin_size = 0;
2038 rfc.max_transmit = 0;
2039 rfc.retrans_timeout = 0;
2040 rfc.monitor_timeout = 0;
2041 rfc.max_pdu_size = 0;
2042
63406504
GP		 2043 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
2044 (unsigned long) &rfc);
65c7c491
MH		 2045 break;
2046
2047 case L2CAP_MODE_ERTM:
2048 rfc.mode = L2CAP_MODE_ERTM;
47d1ec61 2049 rfc.max_transmit = chan->max_tx;
f2fcfcd6
GP		 2050 rfc.retrans_timeout = 0;
2051 rfc.monitor_timeout = 0;
c8f79162
AE		 2052
2053 size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
2054 L2CAP_EXT_HDR_SIZE -
2055 L2CAP_SDULEN_SIZE -
2056 L2CAP_FCS_SIZE);
2057 rfc.max_pdu_size = cpu_to_le16(size);
f2fcfcd6 2058
6327eb98
AE		 2059 l2cap_txwin_setup(chan);
2060
2061 rfc.txwin_size = min_t(u16, chan->tx_win,
2062 L2CAP_DEFAULT_TX_WINDOW);
f2fcfcd6 2063
63406504
GP		 2064 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
2065 (unsigned long) &rfc);
2066
f89cef09
AE		 2067 if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
2068 l2cap_add_opt_efs(&ptr, chan);
2069
8c1d787b 2070 if (!(chan->conn->feat_mask & L2CAP_FEAT_FCS))
fcc203c3
GP		 2071 break;
2072
47d1ec61 2073 if (chan->fcs == L2CAP_FCS_NONE ||
c1360a1c 2074 test_bit(CONF_NO_FCS_RECV, &chan->conf_state)) {
47d1ec61
GP		 2075 chan->fcs = L2CAP_FCS_NONE;
2076 l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, chan->fcs);
fcc203c3 2077 }
6327eb98
AE		 2078
2079 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
2080 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
2081 chan->tx_win);
f2fcfcd6
GP		 2082 break;
2083
2084 case L2CAP_MODE_STREAMING:
2085 rfc.mode = L2CAP_MODE_STREAMING;
2086 rfc.txwin_size = 0;
2087 rfc.max_transmit = 0;
2088 rfc.retrans_timeout = 0;
2089 rfc.monitor_timeout = 0;
c8f79162
AE		 2090
2091 size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
2092 L2CAP_EXT_HDR_SIZE -
2093 L2CAP_SDULEN_SIZE -
2094 L2CAP_FCS_SIZE);
2095 rfc.max_pdu_size = cpu_to_le16(size);
65c7c491 2096
63406504
GP		 2097 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
2098 (unsigned long) &rfc);
2099
f89cef09
AE		 2100 if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
2101 l2cap_add_opt_efs(&ptr, chan);
2102
8c1d787b 2103 if (!(chan->conn->feat_mask & L2CAP_FEAT_FCS))
fcc203c3
GP		 2104 break;
2105
47d1ec61 2106 if (chan->fcs == L2CAP_FCS_NONE ||
c1360a1c 2107 test_bit(CONF_NO_FCS_RECV, &chan->conf_state)) {
47d1ec61
GP		 2108 chan->fcs = L2CAP_FCS_NONE;
2109 l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, chan->fcs);
fcc203c3 2110 }
65c7c491
MH		 2111 break;
2112 }
1da177e4 2113
fe4128e0 2114 req->dcid = cpu_to_le16(chan->dcid);
aca3192c 2115 req->flags = cpu_to_le16(0);
1da177e4
LT		 2116
2117 return ptr - data;
2118}
2119
73ffa904 2120static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data)
1da177e4 2121{
5dee9e7c
MH		 2122 struct l2cap_conf_rsp *rsp = data;
2123 void *ptr = rsp->data;
73ffa904
GP		 2124 void *req = chan->conf_req;
2125 int len = chan->conf_len;
5dee9e7c
MH		 2126 int type, hint, olen;
2127 unsigned long val;
6464f35f 2128 struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
42dceae2
AE		 2129 struct l2cap_conf_efs efs;
2130 u8 remote_efs = 0;
861d6882 2131 u16 mtu = L2CAP_DEFAULT_MTU;
5dee9e7c 2132 u16 result = L2CAP_CONF_SUCCESS;
c8f79162 2133 u16 size;
1da177e4 2134
73ffa904 2135 BT_DBG("chan %p", chan);
820ae1b8 2136
5dee9e7c
MH		 2137 while (len >= L2CAP_CONF_OPT_SIZE) {
2138 len -= l2cap_get_conf_opt(&req, &type, &olen, &val);
1da177e4 2139
589d2746 2140 hint = type & L2CAP_CONF_HINT;
47ec1dcd 2141 type &= L2CAP_CONF_MASK;
5dee9e7c
MH		 2142
2143 switch (type) {
2144 case L2CAP_CONF_MTU:
861d6882 2145 mtu = val;
5dee9e7c
MH		 2146 break;
2147
2148 case L2CAP_CONF_FLUSH_TO:
0c1bc5c6 2149 chan->flush_to = val;
5dee9e7c
MH		 2150 break;
2151
2152 case L2CAP_CONF_QOS:
2153 break;
2154
6464f35f
MH		 2155 case L2CAP_CONF_RFC:
2156 if (olen == sizeof(rfc))
2157 memcpy(&rfc, (void *) val, olen);
2158 break;
2159
fcc203c3
GP		 2160 case L2CAP_CONF_FCS:
2161 if (val == L2CAP_FCS_NONE)
c1360a1c 2162 set_bit(CONF_NO_FCS_RECV, &chan->conf_state);
42dceae2 2163 break;
fcc203c3 2164
42dceae2
AE		 2165 case L2CAP_CONF_EFS:
2166 remote_efs = 1;
2167 if (olen == sizeof(efs))
2168 memcpy(&efs, (void *) val, olen);
fcc203c3
GP		 2169 break;
2170
6327eb98
AE		 2171 case L2CAP_CONF_EWS:
2172 if (!enable_hs)
2173 return -ECONNREFUSED;
fcc203c3 2174
6327eb98
AE		 2175 set_bit(FLAG_EXT_CTRL, &chan->flags);
2176 set_bit(CONF_EWS_RECV, &chan->conf_state);
836be934 2177 chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
6327eb98 2178 chan->remote_tx_win = val;
fcc203c3
GP		 2179 break;
2180
5dee9e7c
MH		 2181 default:
2182 if (hint)
2183 break;
2184
2185 result = L2CAP_CONF_UNKNOWN;
2186 *((u8 *) ptr++) = type;
2187 break;
2188 }
2189 }
2190
73ffa904 2191 if (chan->num_conf_rsp || chan->num_conf_req > 1)
f2fcfcd6
GP		 2192 goto done;
2193
0c1bc5c6 2194 switch (chan->mode) {
f2fcfcd6
GP		 2195 case L2CAP_MODE_STREAMING:
2196 case L2CAP_MODE_ERTM:
c1360a1c 2197 if (!test_bit(CONF_STATE2_DEVICE, &chan->conf_state)) {
0c1bc5c6 2198 chan->mode = l2cap_select_mode(rfc.mode,
8c1d787b 2199 chan->conn->feat_mask);
85eb53c6
GP		 2200 break;
2201 }
2202
42dceae2
AE		 2203 if (remote_efs) {
2204 if (__l2cap_efs_supported(chan))
2205 set_bit(FLAG_EFS_ENABLE, &chan->flags);
2206 else
2207 return -ECONNREFUSED;
2208 }
2209
0c1bc5c6 2210 if (chan->mode != rfc.mode)
f2fcfcd6 2211 return -ECONNREFUSED;
742e519b 2212
f2fcfcd6 2213 break;
f2fcfcd6
GP		 2214 }
2215
2216done:
0c1bc5c6 2217 if (chan->mode != rfc.mode) {
f2fcfcd6 2218 result = L2CAP_CONF_UNACCEPT;
0c1bc5c6 2219 rfc.mode = chan->mode;
f2fcfcd6 2220
73ffa904 2221 if (chan->num_conf_rsp == 1)
f2fcfcd6
GP		 2222 return -ECONNREFUSED;
2223
2224 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2225 sizeof(rfc), (unsigned long) &rfc);
2226 }
2227
5dee9e7c
MH		 2228 if (result == L2CAP_CONF_SUCCESS) {
2229 /* Configure output options and let the other side know
2230 * which ones we don't like. */
2231
f2fcfcd6
GP		 2232 if (mtu < L2CAP_DEFAULT_MIN_MTU)
2233 result = L2CAP_CONF_UNACCEPT;
2234 else {
0c1bc5c6 2235 chan->omtu = mtu;
c1360a1c 2236 set_bit(CONF_MTU_DONE, &chan->conf_state);
f2fcfcd6 2237 }
0c1bc5c6 2238 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->omtu);
6464f35f 2239
42dceae2
AE		 2240 if (remote_efs) {
2241 if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
2242 efs.stype != L2CAP_SERV_NOTRAFIC &&
2243 efs.stype != chan->local_stype) {
2244
2245 result = L2CAP_CONF_UNACCEPT;
2246
2247 if (chan->num_conf_req >= 1)
2248 return -ECONNREFUSED;
2249
2250 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
3e6b3b95 2251 sizeof(efs),
42dceae2 2252 (unsigned long) &efs);
0e8b207e 2253 } else {
3e6b3b95 2254 /* Send PENDING Conf Rsp */
0e8b207e
AE		 2255 result = L2CAP_CONF_PENDING;
2256 set_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
42dceae2
AE		 2257 }
2258 }
2259
f2fcfcd6
GP		 2260 switch (rfc.mode) {
2261 case L2CAP_MODE_BASIC:
47d1ec61 2262 chan->fcs = L2CAP_FCS_NONE;
c1360a1c 2263 set_bit(CONF_MODE_DONE, &chan->conf_state);
f2fcfcd6
GP		 2264 break;
2265
2266 case L2CAP_MODE_ERTM:
6327eb98
AE		 2267 if (!test_bit(CONF_EWS_RECV, &chan->conf_state))
2268 chan->remote_tx_win = rfc.txwin_size;
2269 else
2270 rfc.txwin_size = L2CAP_DEFAULT_TX_WINDOW;
86b1b263 2271
2c03a7a4 2272 chan->remote_max_tx = rfc.max_transmit;
1c762159 2273
c8f79162
AE		 2274 size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
2275 chan->conn->mtu -
2276 L2CAP_EXT_HDR_SIZE -
2277 L2CAP_SDULEN_SIZE -
2278 L2CAP_FCS_SIZE);
2279 rfc.max_pdu_size = cpu_to_le16(size);
2280 chan->remote_mps = size;
f2fcfcd6 2281
10467e9e
GP		 2282 rfc.retrans_timeout =
2283 le16_to_cpu(L2CAP_DEFAULT_RETRANS_TO);
2284 rfc.monitor_timeout =
2285 le16_to_cpu(L2CAP_DEFAULT_MONITOR_TO);
f2fcfcd6 2286
c1360a1c 2287 set_bit(CONF_MODE_DONE, &chan->conf_state);
68ae6639
GP		 2288
2289 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2290 sizeof(rfc), (unsigned long) &rfc);
2291
42dceae2
AE		 2292 if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
2293 chan->remote_id = efs.id;
2294 chan->remote_stype = efs.stype;
2295 chan->remote_msdu = le16_to_cpu(efs.msdu);
2296 chan->remote_flush_to =
2297 le32_to_cpu(efs.flush_to);
2298 chan->remote_acc_lat =
2299 le32_to_cpu(efs.acc_lat);
2300 chan->remote_sdu_itime =
2301 le32_to_cpu(efs.sdu_itime);
2302 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
2303 sizeof(efs), (unsigned long) &efs);
2304 }
f2fcfcd6
GP		 2305 break;
2306
2307 case L2CAP_MODE_STREAMING:
c8f79162
AE		 2308 size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
2309 chan->conn->mtu -
2310 L2CAP_EXT_HDR_SIZE -
2311 L2CAP_SDULEN_SIZE -
2312 L2CAP_FCS_SIZE);
2313 rfc.max_pdu_size = cpu_to_le16(size);
2314 chan->remote_mps = size;
f2fcfcd6 2315
c1360a1c 2316 set_bit(CONF_MODE_DONE, &chan->conf_state);
68ae6639
GP		 2317
2318 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2319 sizeof(rfc), (unsigned long) &rfc);
2320
f2fcfcd6
GP		 2321 break;
2322
2323 default:
5dee9e7c 2324 result = L2CAP_CONF_UNACCEPT;
5dee9e7c 2325
6464f35f 2326 memset(&rfc, 0, sizeof(rfc));
0c1bc5c6 2327 rfc.mode = chan->mode;
f2fcfcd6 2328 }
6464f35f 2329
f2fcfcd6 2330 if (result == L2CAP_CONF_SUCCESS)
c1360a1c 2331 set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
f2fcfcd6 2332 }
fe4128e0 2333 rsp->scid = cpu_to_le16(chan->dcid);
5dee9e7c
MH		 2334 rsp->result = cpu_to_le16(result);
2335 rsp->flags = cpu_to_le16(0x0000);
2336
2337 return ptr - data;
1da177e4
LT		 2338}
2339
b4450035 2340static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, void *data, u16 *result)
f2fcfcd6 2341{
f2fcfcd6
GP		 2342 struct l2cap_conf_req *req = data;
2343 void *ptr = req->data;
2344 int type, olen;
2345 unsigned long val;
2346 struct l2cap_conf_rfc rfc;
66af7aaf 2347 struct l2cap_conf_efs efs;
f2fcfcd6 2348
fe4128e0 2349 BT_DBG("chan %p, rsp %p, len %d, req %p", chan, rsp, len, data);
f2fcfcd6
GP		 2350
2351 while (len >= L2CAP_CONF_OPT_SIZE) {
2352 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
2353
2354 switch (type) {
2355 case L2CAP_CONF_MTU:
2356 if (val < L2CAP_DEFAULT_MIN_MTU) {
2357 *result = L2CAP_CONF_UNACCEPT;
0c1bc5c6 2358 chan->imtu = L2CAP_DEFAULT_MIN_MTU;
f2fcfcd6 2359 } else
0c1bc5c6
GP		 2360 chan->imtu = val;
2361 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu);
f2fcfcd6
GP		 2362 break;
2363
2364 case L2CAP_CONF_FLUSH_TO:
0c1bc5c6 2365 chan->flush_to = val;
f2fcfcd6 2366 l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
0c1bc5c6 2367 2, chan->flush_to);
f2fcfcd6
GP		 2368 break;
2369
2370 case L2CAP_CONF_RFC:
2371 if (olen == sizeof(rfc))
2372 memcpy(&rfc, (void *)val, olen);
2373
c1360a1c 2374 if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state) &&
0c1bc5c6 2375 rfc.mode != chan->mode)
f2fcfcd6
GP		 2376 return -ECONNREFUSED;
2377
47d1ec61 2378 chan->fcs = 0;
f2fcfcd6
GP		 2379
2380 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2381 sizeof(rfc), (unsigned long) &rfc);
2382 break;
6327eb98
AE		 2383
2384 case L2CAP_CONF_EWS:
2385 chan->tx_win = min_t(u16, val,
2386 L2CAP_DEFAULT_EXT_WINDOW);
3e6b3b95
GP		 2387 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
2388 chan->tx_win);
6327eb98 2389 break;
66af7aaf
AE		 2390
2391 case L2CAP_CONF_EFS:
2392 if (olen == sizeof(efs))
2393 memcpy(&efs, (void *)val, olen);
2394
2395 if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
2396 efs.stype != L2CAP_SERV_NOTRAFIC &&
2397 efs.stype != chan->local_stype)
2398 return -ECONNREFUSED;
2399
2400 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
2401 sizeof(efs), (unsigned long) &efs);
2402 break;
f2fcfcd6
GP		 2403 }
2404 }
2405
0c1bc5c6 2406 if (chan->mode == L2CAP_MODE_BASIC && chan->mode != rfc.mode)
6c2ea7a8
GP		 2407 return -ECONNREFUSED;
2408
0c1bc5c6 2409 chan->mode = rfc.mode;
6c2ea7a8 2410
0e8b207e 2411 if (*result == L2CAP_CONF_SUCCESS || *result == L2CAP_CONF_PENDING) {
f2fcfcd6
GP		 2412 switch (rfc.mode) {
2413 case L2CAP_MODE_ERTM:
47d1ec61
GP		 2414 chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
2415 chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
2416 chan->mps = le16_to_cpu(rfc.max_pdu_size);
66af7aaf
AE		 2417
2418 if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
2419 chan->local_msdu = le16_to_cpu(efs.msdu);
2420 chan->local_sdu_itime =
2421 le32_to_cpu(efs.sdu_itime);
2422 chan->local_acc_lat = le32_to_cpu(efs.acc_lat);
2423 chan->local_flush_to =
2424 le32_to_cpu(efs.flush_to);
2425 }
f2fcfcd6 2426 break;
66af7aaf 2427
f2fcfcd6 2428 case L2CAP_MODE_STREAMING:
47d1ec61 2429 chan->mps = le16_to_cpu(rfc.max_pdu_size);
f2fcfcd6
GP		 2430 }
2431 }
2432
fe4128e0 2433 req->dcid = cpu_to_le16(chan->dcid);
f2fcfcd6
GP		 2434 req->flags = cpu_to_le16(0x0000);
2435
2436 return ptr - data;
2437}
2438
fe4128e0 2439static int l2cap_build_conf_rsp(struct l2cap_chan *chan, void *data, u16 result, u16 flags)
1da177e4
LT		 2440{
2441 struct l2cap_conf_rsp *rsp = data;
2442 void *ptr = rsp->data;
1da177e4 2443
fe4128e0 2444 BT_DBG("chan %p", chan);
1da177e4 2445
fe4128e0 2446 rsp->scid = cpu_to_le16(chan->dcid);
5dee9e7c 2447 rsp->result = cpu_to_le16(result);
aca3192c 2448 rsp->flags = cpu_to_le16(flags);
1da177e4
LT		 2449
2450 return ptr - data;
2451}
2452
8c1d787b 2453void __l2cap_connect_rsp_defer(struct l2cap_chan *chan)
710f9b0a
GP		 2454{
2455 struct l2cap_conn_rsp rsp;
8c1d787b 2456 struct l2cap_conn *conn = chan->conn;
710f9b0a
GP		 2457 u8 buf[128];
2458
fe4128e0
GP		 2459 rsp.scid = cpu_to_le16(chan->dcid);
2460 rsp.dcid = cpu_to_le16(chan->scid);
710f9b0a
GP		 2461 rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
2462 rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
2463 l2cap_send_cmd(conn, chan->ident,
2464 L2CAP_CONN_RSP, sizeof(rsp), &rsp);
2465
c1360a1c 2466 if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
710f9b0a
GP		 2467 return;
2468
710f9b0a
GP		 2469 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2470 l2cap_build_conf_req(chan, buf), buf);
2471 chan->num_conf_req++;
2472}
2473
47d1ec61 2474static void l2cap_conf_rfc_get(struct l2cap_chan *chan, void *rsp, int len)
7b1c0049 2475{
7b1c0049
GP		 2476 int type, olen;
2477 unsigned long val;
2478 struct l2cap_conf_rfc rfc;
2479
47d1ec61 2480 BT_DBG("chan %p, rsp %p, len %d", chan, rsp, len);
7b1c0049 2481
0c1bc5c6 2482 if ((chan->mode != L2CAP_MODE_ERTM) && (chan->mode != L2CAP_MODE_STREAMING))
7b1c0049
GP		 2483 return;
2484
2485 while (len >= L2CAP_CONF_OPT_SIZE) {
2486 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
2487
2488 switch (type) {
2489 case L2CAP_CONF_RFC:
2490 if (olen == sizeof(rfc))
2491 memcpy(&rfc, (void *)val, olen);
2492 goto done;
2493 }
2494 }
2495
2496done:
2497 switch (rfc.mode) {
2498 case L2CAP_MODE_ERTM:
47d1ec61
GP		 2499 chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
2500 chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
2501 chan->mps = le16_to_cpu(rfc.max_pdu_size);
7b1c0049
GP		 2502 break;
2503 case L2CAP_MODE_STREAMING:
47d1ec61 2504 chan->mps = le16_to_cpu(rfc.max_pdu_size);
7b1c0049
GP		 2505 }
2506}
2507
4e8402a3
MH		 2508static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2509{
e2fd318e 2510 struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data;
4e8402a3 2511
e2fd318e 2512 if (rej->reason != L2CAP_REJ_NOT_UNDERSTOOD)
4e8402a3
MH		 2513 return 0;
2514
2515 if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
2516 cmd->ident == conn->info_ident) {
4e8402a3 2517 del_timer(&conn->info_timer);
984947dc
MH		 2518
2519 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
e1027a7c 2520 conn->info_ident = 0;
984947dc 2521
4e8402a3
MH		 2522 l2cap_conn_start(conn);
2523 }
2524
2525 return 0;
2526}
2527
1da177e4
LT		 2528static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2529{
1da177e4
LT		 2530 struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
2531 struct l2cap_conn_rsp rsp;
23691d75 2532 struct l2cap_chan *chan = NULL, *pchan;
d793fe8c 2533 struct sock *parent, *sk = NULL;
e7c29cb1 2534 int result, status = L2CAP_CS_NO_INFO;
1da177e4
LT		 2535
2536 u16 dcid = 0, scid = __le16_to_cpu(req->scid);
e7c29cb1 2537 __le16 psm = req->psm;
1da177e4
LT		 2538
2539 BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid);
2540
2541 /* Check if we have socket listening on psm */
23691d75
GP		 2542 pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, conn->src);
2543 if (!pchan) {
1da177e4
LT		 2544 result = L2CAP_CR_BAD_PSM;
2545 goto sendresp;
2546 }
2547
23691d75
GP		 2548 parent = pchan->sk;
2549
e0f0cb56
GP		 2550 bh_lock_sock(parent);
2551
e7c29cb1
MH		 2552 /* Check if the ACL is secure enough (if not SDP) */
2553 if (psm != cpu_to_le16(0x0001) &&
2554 !hci_conn_check_link_mode(conn->hcon)) {
9f5a0d7b 2555 conn->disc_reason = HCI_ERROR_AUTH_FAILURE;
e7c29cb1
MH		 2556 result = L2CAP_CR_SEC_BLOCK;
2557 goto response;
2558 }
2559
1da177e4
LT		 2560 result = L2CAP_CR_NO_MEM;
2561
2562 /* Check for backlog size */
2563 if (sk_acceptq_is_full(parent)) {
8e87d142 2564 BT_DBG("backlog full %d", parent->sk_ack_backlog);
1da177e4
LT		 2565 goto response;
2566 }
2567
80808e43
GP		 2568 chan = pchan->ops->new_connection(pchan->data);
2569 if (!chan)
1da177e4
LT		 2570 goto response;
2571
80808e43
GP		 2572 sk = chan->sk;
2573
baa7e1fa 2574 write_lock_bh(&conn->chan_lock);
1da177e4
LT		 2575
2576 /* Check if we already have channel with that dcid */
baa7e1fa
GP		 2577 if (__l2cap_get_chan_by_dcid(conn, scid)) {
2578 write_unlock_bh(&conn->chan_lock);
1da177e4 2579 sock_set_flag(sk, SOCK_ZAPPED);
ba3bd0ee 2580 chan->ops->close(chan->data);
1da177e4
LT		 2581 goto response;
2582 }
2583
2584 hci_conn_hold(conn->hcon);
2585
1da177e4
LT		 2586 bacpy(&bt_sk(sk)->src, conn->src);
2587 bacpy(&bt_sk(sk)->dst, conn->dst);
fe4128e0
GP		 2588 chan->psm = psm;
2589 chan->dcid = scid;
1da177e4 2590
d1010240
GP		 2591 bt_accept_enqueue(parent, sk);
2592
48454079
GP		 2593 __l2cap_chan_add(conn, chan);
2594
fe4128e0 2595 dcid = chan->scid;
1da177e4 2596
c9b66675 2597 __set_chan_timer(chan, sk->sk_sndtimeo);
1da177e4 2598
fc7f8a7e 2599 chan->ident = cmd->ident;
1da177e4 2600
984947dc 2601 if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
d45fc423 2602 if (l2cap_chan_check_security(chan)) {
f66dc81f 2603 if (bt_sk(sk)->defer_setup) {
89bc500e 2604 l2cap_state_change(chan, BT_CONNECT2);
f66dc81f
MH		 2605 result = L2CAP_CR_PEND;
2606 status = L2CAP_CS_AUTHOR_PEND;
2607 parent->sk_data_ready(parent, 0);
2608 } else {
89bc500e 2609 l2cap_state_change(chan, BT_CONFIG);
f66dc81f
MH		 2610 result = L2CAP_CR_SUCCESS;
2611 status = L2CAP_CS_NO_INFO;
2612 }
79d554a6 2613 } else {
89bc500e 2614 l2cap_state_change(chan, BT_CONNECT2);
79d554a6
MH		 2615 result = L2CAP_CR_PEND;
2616 status = L2CAP_CS_AUTHEN_PEND;
2617 }
2618 } else {
89bc500e 2619 l2cap_state_change(chan, BT_CONNECT2);
79d554a6
MH		 2620 result = L2CAP_CR_PEND;
2621 status = L2CAP_CS_NO_INFO;
1da177e4
LT		 2622 }
2623
baa7e1fa 2624 write_unlock_bh(&conn->chan_lock);
1da177e4
LT		 2625
2626response:
2627 bh_unlock_sock(parent);
2628
2629sendresp:
aca3192c
YH		 2630 rsp.scid = cpu_to_le16(scid);
2631 rsp.dcid = cpu_to_le16(dcid);
2632 rsp.result = cpu_to_le16(result);
2633 rsp.status = cpu_to_le16(status);
1da177e4 2634 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
79d554a6
MH		 2635
2636 if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
2637 struct l2cap_info_req info;
2638 info.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
2639
2640 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
2641 conn->info_ident = l2cap_get_ident(conn);
2642
2643 mod_timer(&conn->info_timer, jiffies +
2644 msecs_to_jiffies(L2CAP_INFO_TIMEOUT));
2645
2646 l2cap_send_cmd(conn, conn->info_ident,
2647 L2CAP_INFO_REQ, sizeof(info), &info);
2648 }
2649
c1360a1c 2650 if (chan && !test_bit(CONF_REQ_SENT, &chan->conf_state) &&
e9aeb2dd
GP		 2651 result == L2CAP_CR_SUCCESS) {
2652 u8 buf[128];
c1360a1c 2653 set_bit(CONF_REQ_SENT, &chan->conf_state);
e9aeb2dd 2654 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
73ffa904
GP		 2655 l2cap_build_conf_req(chan, buf), buf);
2656 chan->num_conf_req++;
e9aeb2dd
GP		 2657 }
2658
1da177e4
LT		 2659 return 0;
2660}
2661
2662static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2663{
2664 struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
2665 u16 scid, dcid, result, status;
48454079 2666 struct l2cap_chan *chan;
1da177e4
LT		 2667 struct sock *sk;
2668 u8 req[128];
2669
2670 scid = __le16_to_cpu(rsp->scid);
2671 dcid = __le16_to_cpu(rsp->dcid);
2672 result = __le16_to_cpu(rsp->result);
2673 status = __le16_to_cpu(rsp->status);
2674
2675 BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", dcid, scid, result, status);
2676
2677 if (scid) {
baa7e1fa 2678 chan = l2cap_get_chan_by_scid(conn, scid);
48454079 2679 if (!chan)
57d3b22b 2680 return -EFAULT;
1da177e4 2681 } else {
baa7e1fa 2682 chan = l2cap_get_chan_by_ident(conn, cmd->ident);
48454079 2683 if (!chan)
57d3b22b 2684 return -EFAULT;
1da177e4
LT		 2685 }
2686
48454079
GP		 2687 sk = chan->sk;
2688
1da177e4
LT		 2689 switch (result) {
2690 case L2CAP_CR_SUCCESS:
89bc500e 2691 l2cap_state_change(chan, BT_CONFIG);
fc7f8a7e 2692 chan->ident = 0;
fe4128e0 2693 chan->dcid = dcid;
c1360a1c 2694 clear_bit(CONF_CONNECT_PEND, &chan->conf_state);
6a8d3010 2695
c1360a1c 2696 if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
e9aeb2dd
GP		 2697 break;
2698
1da177e4 2699 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
73ffa904
GP		 2700 l2cap_build_conf_req(chan, req), req);
2701 chan->num_conf_req++;
1da177e4
LT		 2702 break;
2703
2704 case L2CAP_CR_PEND:
c1360a1c 2705 set_bit(CONF_CONNECT_PEND, &chan->conf_state);
1da177e4
LT		 2706 break;
2707
2708 default:
a49184c2
AE		 2709 /* don't delete l2cap channel if sk is owned by user */
2710 if (sock_owned_by_user(sk)) {
89bc500e 2711 l2cap_state_change(chan, BT_DISCONN);
c9b66675 2712 __clear_chan_timer(chan);
f3f668b0 2713 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
a49184c2
AE		 2714 break;
2715 }
2716
48454079 2717 l2cap_chan_del(chan, ECONNREFUSED);
1da177e4
LT		 2718 break;
2719 }
2720
2721 bh_unlock_sock(sk);
2722 return 0;
2723}
2724
47d1ec61 2725static inline void set_default_fcs(struct l2cap_chan *chan)
8c462b60
MM		 2726{
2727 /* FCS is enabled only in ERTM or streaming mode, if one or both
2728 * sides request it.
2729 */
0c1bc5c6 2730 if (chan->mode != L2CAP_MODE_ERTM && chan->mode != L2CAP_MODE_STREAMING)
47d1ec61 2731 chan->fcs = L2CAP_FCS_NONE;
c1360a1c 2732 else if (!test_bit(CONF_NO_FCS_RECV, &chan->conf_state))
47d1ec61 2733 chan->fcs = L2CAP_FCS_CRC16;
8c462b60
MM		 2734}
2735
88219a0f 2736static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
1da177e4
LT		 2737{
2738 struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
2739 u16 dcid, flags;
2740 u8 rsp[64];
48454079 2741 struct l2cap_chan *chan;
1da177e4 2742 struct sock *sk;
5dee9e7c 2743 int len;
1da177e4
LT		 2744
2745 dcid = __le16_to_cpu(req->dcid);
2746 flags = __le16_to_cpu(req->flags);
2747
2748 BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
2749
baa7e1fa 2750 chan = l2cap_get_chan_by_scid(conn, dcid);
48454079 2751 if (!chan)
1da177e4
LT		 2752 return -ENOENT;
2753
48454079
GP		 2754 sk = chan->sk;
2755
033b1142 2756 if (chan->state != BT_CONFIG && chan->state != BT_CONNECT2) {
e2fd318e
IK		 2757 struct l2cap_cmd_rej_cid rej;
2758
2759 rej.reason = cpu_to_le16(L2CAP_REJ_INVALID_CID);
2760 rej.scid = cpu_to_le16(chan->scid);
2761 rej.dcid = cpu_to_le16(chan->dcid);
df6bd743 2762
df6bd743
GP		 2763 l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ,
2764 sizeof(rej), &rej);
354f60a9 2765 goto unlock;
df6bd743 2766 }
354f60a9 2767
5dee9e7c 2768 /* Reject if config buffer is too small. */
88219a0f 2769 len = cmd_len - sizeof(*req);
7ac28817 2770 if (len < 0 || chan->conf_len + len > sizeof(chan->conf_req)) {
5dee9e7c 2771 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
fe4128e0 2772 l2cap_build_conf_rsp(chan, rsp,
5dee9e7c
MH		 2773 L2CAP_CONF_REJECT, flags), rsp);
2774 goto unlock;
2775 }
2776
2777 /* Store config. */
73ffa904
GP		 2778 memcpy(chan->conf_req + chan->conf_len, req->data, len);
2779 chan->conf_len += len;
1da177e4
LT		 2780
2781 if (flags & 0x0001) {
2782 /* Incomplete config. Send empty response. */
2783 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
fe4128e0 2784 l2cap_build_conf_rsp(chan, rsp,
5dee9e7c 2785 L2CAP_CONF_SUCCESS, 0x0001), rsp);
1da177e4
LT		 2786 goto unlock;
2787 }
2788
2789 /* Complete config. */
73ffa904 2790 len = l2cap_parse_conf_req(chan, rsp);
f2fcfcd6 2791 if (len < 0) {
e92c8e70 2792 l2cap_send_disconn_req(conn, chan, ECONNRESET);
1da177e4 2793 goto unlock;
f2fcfcd6 2794 }
1da177e4 2795
5dee9e7c 2796 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
73ffa904 2797 chan->num_conf_rsp++;
5dee9e7c 2798
5dee9e7c 2799 /* Reset config buffer. */
73ffa904 2800 chan->conf_len = 0;
5dee9e7c 2801
c1360a1c 2802 if (!test_bit(CONF_OUTPUT_DONE, &chan->conf_state))
876d9484
MH		 2803 goto unlock;
2804
c1360a1c 2805 if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
47d1ec61 2806 set_default_fcs(chan);
fcc203c3 2807
89bc500e 2808 l2cap_state_change(chan, BT_CONNECTED);
e90bac06 2809
42e5c802
GP		 2810 chan->next_tx_seq = 0;
2811 chan->expected_tx_seq = 0;
58d35f87 2812 skb_queue_head_init(&chan->tx_q);
0c1bc5c6 2813 if (chan->mode == L2CAP_MODE_ERTM)
525cd185 2814 l2cap_ertm_init(chan);
0565c1c2 2815
1da177e4 2816 l2cap_chan_ready(sk);
876d9484
MH		 2817 goto unlock;
2818 }
2819
c1360a1c 2820 if (!test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) {
79d554a6 2821 u8 buf[64];
1da177e4 2822 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
73ffa904
GP		 2823 l2cap_build_conf_req(chan, buf), buf);
2824 chan->num_conf_req++;
1da177e4
LT		 2825 }
2826
0e8b207e
AE		 2827 /* Got Conf Rsp PENDING from remote side and asume we sent
2828 Conf Rsp PENDING in the code above */
2829 if (test_bit(CONF_REM_CONF_PEND, &chan->conf_state) &&
2830 test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
2831
2832 /* check compatibility */
2833
2834 clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
2835 set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
2836
2837 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
3e6b3b95 2838 l2cap_build_conf_rsp(chan, rsp,
0e8b207e
AE		 2839 L2CAP_CONF_SUCCESS, 0x0000), rsp);
2840 }
2841
1da177e4
LT		 2842unlock:
2843 bh_unlock_sock(sk);
2844 return 0;
2845}
2846
2847static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2848{
2849 struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
2850 u16 scid, flags, result;
48454079 2851 struct l2cap_chan *chan;
1da177e4 2852 struct sock *sk;
7b1c0049 2853 int len = cmd->len - sizeof(*rsp);
1da177e4
LT		 2854
2855 scid = __le16_to_cpu(rsp->scid);
2856 flags = __le16_to_cpu(rsp->flags);
2857 result = __le16_to_cpu(rsp->result);
2858
af05b30b
GP		 2859 BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x",
2860 scid, flags, result);
1da177e4 2861
baa7e1fa 2862 chan = l2cap_get_chan_by_scid(conn, scid);
48454079 2863 if (!chan)
1da177e4
LT		 2864 return 0;
2865
48454079
GP		 2866 sk = chan->sk;
2867
1da177e4
LT		 2868 switch (result) {
2869 case L2CAP_CONF_SUCCESS:
47d1ec61 2870 l2cap_conf_rfc_get(chan, rsp->data, len);
0e8b207e 2871 clear_bit(CONF_REM_CONF_PEND, &chan->conf_state);
1da177e4
LT		 2872 break;
2873
0e8b207e
AE		 2874 case L2CAP_CONF_PENDING:
2875 set_bit(CONF_REM_CONF_PEND, &chan->conf_state);
2876
2877 if (test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
2878 char buf[64];
2879
2880 len = l2cap_parse_conf_rsp(chan, rsp->data, len,
2881 buf, &result);
2882 if (len < 0) {
2883 l2cap_send_disconn_req(conn, chan, ECONNRESET);
2884 goto done;
2885 }
2886
2887 /* check compatibility */
2888
2889 clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
2890 set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
2891
2892 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
3e6b3b95 2893 l2cap_build_conf_rsp(chan, buf,
0e8b207e
AE		 2894 L2CAP_CONF_SUCCESS, 0x0000), buf);
2895 }
2896 goto done;
2897
1da177e4 2898 case L2CAP_CONF_UNACCEPT:
73ffa904 2899 if (chan->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
f2fcfcd6
GP		 2900 char req[64];
2901
c2c77ec8 2902 if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) {
e92c8e70 2903 l2cap_send_disconn_req(conn, chan, ECONNRESET);
c2c77ec8
AE		 2904 goto done;
2905 }
2906
f2fcfcd6
GP		 2907 /* throw out any old stored conf requests */
2908 result = L2CAP_CONF_SUCCESS;
b4450035
GP		 2909 len = l2cap_parse_conf_rsp(chan, rsp->data, len,
2910 req, &result);
f2fcfcd6 2911 if (len < 0) {
e92c8e70 2912 l2cap_send_disconn_req(conn, chan, ECONNRESET);
f2fcfcd6
GP		 2913 goto done;
2914 }
2915
2916 l2cap_send_cmd(conn, l2cap_get_ident(conn),
2917 L2CAP_CONF_REQ, len, req);
73ffa904 2918 chan->num_conf_req++;
f2fcfcd6
GP		 2919 if (result != L2CAP_CONF_SUCCESS)
2920 goto done;
2921 break;
1da177e4
LT		 2922 }
2923
8e87d142 2924 default:
b1235d79 2925 sk->sk_err = ECONNRESET;
f3f668b0 2926 __set_chan_timer(chan, L2CAP_DISC_REJ_TIMEOUT);
e92c8e70 2927 l2cap_send_disconn_req(conn, chan, ECONNRESET);
1da177e4
LT		 2928 goto done;
2929 }
2930
2931 if (flags & 0x01)
2932 goto done;
2933
c1360a1c 2934 set_bit(CONF_INPUT_DONE, &chan->conf_state);
1da177e4 2935
c1360a1c 2936 if (test_bit(CONF_OUTPUT_DONE, &chan->conf_state)) {
47d1ec61 2937 set_default_fcs(chan);
fcc203c3 2938
89bc500e 2939 l2cap_state_change(chan, BT_CONNECTED);
42e5c802
GP		 2940 chan->next_tx_seq = 0;
2941 chan->expected_tx_seq = 0;
58d35f87 2942 skb_queue_head_init(&chan->tx_q);
0c1bc5c6 2943 if (chan->mode == L2CAP_MODE_ERTM)
525cd185 2944 l2cap_ertm_init(chan);
0565c1c2 2945
1da177e4
LT		 2946 l2cap_chan_ready(sk);
2947 }
2948
2949done:
2950 bh_unlock_sock(sk);
2951 return 0;
2952}
2953
2954static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2955{
2956 struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
2957 struct l2cap_disconn_rsp rsp;
2958 u16 dcid, scid;
48454079 2959 struct l2cap_chan *chan;
1da177e4
LT		 2960 struct sock *sk;
2961
2962 scid = __le16_to_cpu(req->scid);
2963 dcid = __le16_to_cpu(req->dcid);
2964
2965 BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
2966
baa7e1fa 2967 chan = l2cap_get_chan_by_scid(conn, dcid);
48454079 2968 if (!chan)
1da177e4
LT		 2969 return 0;
2970
48454079
GP		 2971 sk = chan->sk;
2972
fe4128e0
GP		 2973 rsp.dcid = cpu_to_le16(chan->scid);
2974 rsp.scid = cpu_to_le16(chan->dcid);
1da177e4
LT		 2975 l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
2976
2977 sk->sk_shutdown = SHUTDOWN_MASK;
2978
a49184c2
AE		 2979 /* don't delete l2cap channel if sk is owned by user */
2980 if (sock_owned_by_user(sk)) {
89bc500e 2981 l2cap_state_change(chan, BT_DISCONN);
c9b66675 2982 __clear_chan_timer(chan);
f3f668b0 2983 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
a49184c2
AE		 2984 bh_unlock_sock(sk);
2985 return 0;
2986 }
2987
48454079 2988 l2cap_chan_del(chan, ECONNRESET);
1da177e4
LT		 2989 bh_unlock_sock(sk);
2990
ba3bd0ee 2991 chan->ops->close(chan->data);
1da177e4
LT		 2992 return 0;
2993}
2994
2995static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2996{
2997 struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
2998 u16 dcid, scid;
48454079 2999 struct l2cap_chan *chan;
1da177e4
LT		 3000 struct sock *sk;
3001
3002 scid = __le16_to_cpu(rsp->scid);
3003 dcid = __le16_to_cpu(rsp->dcid);
3004
3005 BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
3006
baa7e1fa 3007 chan = l2cap_get_chan_by_scid(conn, scid);
48454079 3008 if (!chan)
1da177e4
LT		 3009 return 0;
3010
48454079
GP		 3011 sk = chan->sk;
3012
a49184c2
AE		 3013 /* don't delete l2cap channel if sk is owned by user */
3014 if (sock_owned_by_user(sk)) {
1ec918ce 3015 l2cap_state_change(chan, BT_DISCONN);
c9b66675 3016 __clear_chan_timer(chan);
f3f668b0 3017 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
a49184c2
AE		 3018 bh_unlock_sock(sk);
3019 return 0;
3020 }
3021
48454079 3022 l2cap_chan_del(chan, 0);
1da177e4
LT		 3023 bh_unlock_sock(sk);
3024
ba3bd0ee 3025 chan->ops->close(chan->data);
1da177e4
LT		 3026 return 0;
3027}
3028
3029static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3030{
3031 struct l2cap_info_req *req = (struct l2cap_info_req *) data;
1da177e4
LT		 3032 u16 type;
3033
3034 type = __le16_to_cpu(req->type);
3035
3036 BT_DBG("type 0x%4.4x", type);
3037
f0709e03
MH		 3038 if (type == L2CAP_IT_FEAT_MASK) {
3039 u8 buf[8];
44dd46de 3040 u32 feat_mask = l2cap_feat_mask;
f0709e03
MH		 3041 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
3042 rsp->type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
3043 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
d1c4a17d 3044 if (!disable_ertm)
fcc203c3
GP		 3045 feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
3046 | L2CAP_FEAT_FCS;
a5fd6f30 3047 if (enable_hs)
6327eb98
AE		 3048 feat_mask |= L2CAP_FEAT_EXT_FLOW
3049 | L2CAP_FEAT_EXT_WINDOW;
a5fd6f30 3050
1b7bf4ed 3051 put_unaligned_le32(feat_mask, rsp->data);
f0709e03
MH		 3052 l2cap_send_cmd(conn, cmd->ident,
3053 L2CAP_INFO_RSP, sizeof(buf), buf);
e1027a7c
MH		 3054 } else if (type == L2CAP_IT_FIXED_CHAN) {
3055 u8 buf[12];
3056 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
50a147cd
MM		 3057
3058 if (enable_hs)
3059 l2cap_fixed_chan[0] |= L2CAP_FC_A2MP;
3060 else
3061 l2cap_fixed_chan[0] &= ~L2CAP_FC_A2MP;
3062
e1027a7c
MH		 3063 rsp->type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
3064 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
c6337ea6 3065 memcpy(rsp->data, l2cap_fixed_chan, sizeof(l2cap_fixed_chan));
e1027a7c
MH		 3066 l2cap_send_cmd(conn, cmd->ident,
3067 L2CAP_INFO_RSP, sizeof(buf), buf);
f0709e03
MH		 3068 } else {
3069 struct l2cap_info_rsp rsp;
3070 rsp.type = cpu_to_le16(type);
3071 rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP);
3072 l2cap_send_cmd(conn, cmd->ident,
3073 L2CAP_INFO_RSP, sizeof(rsp), &rsp);
3074 }
1da177e4
LT		 3075
3076 return 0;
3077}
3078
3079static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3080{
3081 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
3082 u16 type, result;
3083
3084 type = __le16_to_cpu(rsp->type);
3085 result = __le16_to_cpu(rsp->result);
3086
3087 BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
3088
e90165be
AE		 3089 /* L2CAP Info req/rsp are unbound to channels, add extra checks */
3090 if (cmd->ident != conn->info_ident ||
3091 conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)
3092 return 0;
3093
4e8402a3
MH		 3094 del_timer(&conn->info_timer);
3095
adb08ede
VT		 3096 if (result != L2CAP_IR_SUCCESS) {
3097 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3098 conn->info_ident = 0;
3099
3100 l2cap_conn_start(conn);
3101
3102 return 0;
3103 }
3104
984947dc 3105 if (type == L2CAP_IT_FEAT_MASK) {
83985319 3106 conn->feat_mask = get_unaligned_le32(rsp->data);
4e8402a3 3107
47ec1dcd 3108 if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
e1027a7c
MH		 3109 struct l2cap_info_req req;
3110 req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
3111
3112 conn->info_ident = l2cap_get_ident(conn);
3113
3114 l2cap_send_cmd(conn, conn->info_ident,
3115 L2CAP_INFO_REQ, sizeof(req), &req);
3116 } else {
3117 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3118 conn->info_ident = 0;
3119
3120 l2cap_conn_start(conn);
3121 }
3122 } else if (type == L2CAP_IT_FIXED_CHAN) {
984947dc 3123 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
e1027a7c 3124 conn->info_ident = 0;
984947dc
MH		 3125
3126 l2cap_conn_start(conn);
3127 }
4e8402a3 3128
1da177e4
LT		 3129 return 0;
3130}
3131
f94ff6ff
MM		 3132static inline int l2cap_create_channel_req(struct l2cap_conn *conn,
3133 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
3134 void *data)
3135{
3136 struct l2cap_create_chan_req *req = data;
3137 struct l2cap_create_chan_rsp rsp;
3138 u16 psm, scid;
3139
3140 if (cmd_len != sizeof(*req))
3141 return -EPROTO;
3142
3143 if (!enable_hs)
3144 return -EINVAL;
3145
3146 psm = le16_to_cpu(req->psm);
3147 scid = le16_to_cpu(req->scid);
3148
3149 BT_DBG("psm %d, scid %d, amp_id %d", psm, scid, req->amp_id);
3150
3151 /* Placeholder: Always reject */
3152 rsp.dcid = 0;
3153 rsp.scid = cpu_to_le16(scid);
3154 rsp.result = L2CAP_CR_NO_MEM;
3155 rsp.status = L2CAP_CS_NO_INFO;
3156
3157 l2cap_send_cmd(conn, cmd->ident, L2CAP_CREATE_CHAN_RSP,
3158 sizeof(rsp), &rsp);
3159
3160 return 0;
3161}
3162
3163static inline int l2cap_create_channel_rsp(struct l2cap_conn *conn,
3164 struct l2cap_cmd_hdr *cmd, void *data)
3165{
3166 BT_DBG("conn %p", conn);
3167
3168 return l2cap_connect_rsp(conn, cmd, data);
3169}
3170
8d5a04a1
MM		 3171static void l2cap_send_move_chan_rsp(struct l2cap_conn *conn, u8 ident,
3172 u16 icid, u16 result)
3173{
3174 struct l2cap_move_chan_rsp rsp;
3175
3176 BT_DBG("icid %d, result %d", icid, result);
3177
3178 rsp.icid = cpu_to_le16(icid);
3179 rsp.result = cpu_to_le16(result);
3180
3181 l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_RSP, sizeof(rsp), &rsp);
3182}
3183
3184static void l2cap_send_move_chan_cfm(struct l2cap_conn *conn,
3185 struct l2cap_chan *chan, u16 icid, u16 result)
3186{
3187 struct l2cap_move_chan_cfm cfm;
3188 u8 ident;
3189
3190 BT_DBG("icid %d, result %d", icid, result);
3191
3192 ident = l2cap_get_ident(conn);
3193 if (chan)
3194 chan->ident = ident;
3195
3196 cfm.icid = cpu_to_le16(icid);
3197 cfm.result = cpu_to_le16(result);
3198
3199 l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM, sizeof(cfm), &cfm);
3200}
3201
3202static void l2cap_send_move_chan_cfm_rsp(struct l2cap_conn *conn, u8 ident,
3203 u16 icid)
3204{
3205 struct l2cap_move_chan_cfm_rsp rsp;
3206
3207 BT_DBG("icid %d", icid);
3208
3209 rsp.icid = cpu_to_le16(icid);
3210 l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM_RSP, sizeof(rsp), &rsp);
3211}
3212
3213static inline int l2cap_move_channel_req(struct l2cap_conn *conn,
3214 struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data)
3215{
3216 struct l2cap_move_chan_req *req = data;
3217 u16 icid = 0;
3218 u16 result = L2CAP_MR_NOT_ALLOWED;
3219
3220 if (cmd_len != sizeof(*req))
3221 return -EPROTO;
3222
3223 icid = le16_to_cpu(req->icid);
3224
3225 BT_DBG("icid %d, dest_amp_id %d", icid, req->dest_amp_id);
3226
3227 if (!enable_hs)
3228 return -EINVAL;
3229
3230 /* Placeholder: Always refuse */
3231 l2cap_send_move_chan_rsp(conn, cmd->ident, icid, result);
3232
3233 return 0;
3234}
3235
3236static inline int l2cap_move_channel_rsp(struct l2cap_conn *conn,
3237 struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data)
3238{
3239 struct l2cap_move_chan_rsp *rsp = data;
3240 u16 icid, result;
3241
3242 if (cmd_len != sizeof(*rsp))
3243 return -EPROTO;
3244
3245 icid = le16_to_cpu(rsp->icid);
3246 result = le16_to_cpu(rsp->result);
3247
3248 BT_DBG("icid %d, result %d", icid, result);
3249
3250 /* Placeholder: Always unconfirmed */
3251 l2cap_send_move_chan_cfm(conn, NULL, icid, L2CAP_MC_UNCONFIRMED);
3252
3253 return 0;
3254}
3255
3256static inline int l2cap_move_channel_confirm(struct l2cap_conn *conn,
3257 struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data)
3258{
3259 struct l2cap_move_chan_cfm *cfm = data;
3260 u16 icid, result;
3261
3262 if (cmd_len != sizeof(*cfm))
3263 return -EPROTO;
3264
3265 icid = le16_to_cpu(cfm->icid);
3266 result = le16_to_cpu(cfm->result);
3267
3268 BT_DBG("icid %d, result %d", icid, result);
3269
3270 l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid);
3271
3272 return 0;
3273}
3274
3275static inline int l2cap_move_channel_confirm_rsp(struct l2cap_conn *conn,
3276 struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data)
3277{
3278 struct l2cap_move_chan_cfm_rsp *rsp = data;
3279 u16 icid;
3280
3281 if (cmd_len != sizeof(*rsp))
3282 return -EPROTO;
3283
3284 icid = le16_to_cpu(rsp->icid);
3285
3286 BT_DBG("icid %d", icid);
3287
3288 return 0;
3289}
3290
e2174ca4 3291static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency,
de73115a
CT		 3292 u16 to_multiplier)
3293{
3294 u16 max_latency;
3295
3296 if (min > max || min < 6 || max > 3200)
3297 return -EINVAL;
3298
3299 if (to_multiplier < 10 || to_multiplier > 3200)
3300 return -EINVAL;
3301
3302 if (max >= to_multiplier * 8)
3303 return -EINVAL;
3304
3305 max_latency = (to_multiplier * 8 / max) - 1;
3306 if (latency > 499 || latency > max_latency)
3307 return -EINVAL;
3308
3309 return 0;
3310}
3311
3312static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
3313 struct l2cap_cmd_hdr *cmd, u8 *data)
3314{
3315 struct hci_conn *hcon = conn->hcon;
3316 struct l2cap_conn_param_update_req *req;
3317 struct l2cap_conn_param_update_rsp rsp;
3318 u16 min, max, latency, to_multiplier, cmd_len;
2ce603eb 3319 int err;
de73115a
CT		 3320
3321 if (!(hcon->link_mode & HCI_LM_MASTER))
3322 return -EINVAL;
3323
3324 cmd_len = __le16_to_cpu(cmd->len);
3325 if (cmd_len != sizeof(struct l2cap_conn_param_update_req))
3326 return -EPROTO;
3327
3328 req = (struct l2cap_conn_param_update_req *) data;
e2174ca4
GP		 3329 min = __le16_to_cpu(req->min);
3330 max = __le16_to_cpu(req->max);
de73115a
CT		 3331 latency = __le16_to_cpu(req->latency);
3332 to_multiplier = __le16_to_cpu(req->to_multiplier);
3333
3334 BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x",
3335 min, max, latency, to_multiplier);
3336
3337 memset(&rsp, 0, sizeof(rsp));
2ce603eb
CT		 3338
3339 err = l2cap_check_conn_param(min, max, latency, to_multiplier);
3340 if (err)
de73115a
CT		 3341 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_REJECTED);
3342 else
3343 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED);
3344
3345 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP,
3346 sizeof(rsp), &rsp);
3347
2ce603eb
CT		 3348 if (!err)
3349 hci_le_conn_update(hcon, min, max, latency, to_multiplier);
3350
de73115a
CT		 3351 return 0;
3352}
3353
3300d9a9
CT		 3354static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
3355 struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
3356{
3357 int err = 0;
3358
3359 switch (cmd->code) {
3360 case L2CAP_COMMAND_REJ:
3361 l2cap_command_rej(conn, cmd, data);
3362 break;
3363
3364 case L2CAP_CONN_REQ:
3365 err = l2cap_connect_req(conn, cmd, data);
3366 break;
3367
3368 case L2CAP_CONN_RSP:
3369 err = l2cap_connect_rsp(conn, cmd, data);
3370 break;
3371
3372 case L2CAP_CONF_REQ:
3373 err = l2cap_config_req(conn, cmd, cmd_len, data);
3374 break;
3375
3376 case L2CAP_CONF_RSP:
3377 err = l2cap_config_rsp(conn, cmd, data);
3378 break;
3379
3380 case L2CAP_DISCONN_REQ:
3381 err = l2cap_disconnect_req(conn, cmd, data);
3382 break;
3383
3384 case L2CAP_DISCONN_RSP:
3385 err = l2cap_disconnect_rsp(conn, cmd, data);
3386 break;
3387
3388 case L2CAP_ECHO_REQ:
3389 l2cap_send_cmd(conn, cmd->ident, L2CAP_ECHO_RSP, cmd_len, data);
3390 break;
3391
3392 case L2CAP_ECHO_RSP:
3393 break;
3394
3395 case L2CAP_INFO_REQ:
3396 err = l2cap_information_req(conn, cmd, data);
3397 break;
3398
3399 case L2CAP_INFO_RSP:
3400 err = l2cap_information_rsp(conn, cmd, data);
3401 break;
3402
f94ff6ff
MM		 3403 case L2CAP_CREATE_CHAN_REQ:
3404 err = l2cap_create_channel_req(conn, cmd, cmd_len, data);
3405 break;
3406
3407 case L2CAP_CREATE_CHAN_RSP:
3408 err = l2cap_create_channel_rsp(conn, cmd, data);
3409 break;
3410
8d5a04a1
MM		 3411 case L2CAP_MOVE_CHAN_REQ:
3412 err = l2cap_move_channel_req(conn, cmd, cmd_len, data);
3413 break;
3414
3415 case L2CAP_MOVE_CHAN_RSP:
3416 err = l2cap_move_channel_rsp(conn, cmd, cmd_len, data);
3417 break;
3418
3419 case L2CAP_MOVE_CHAN_CFM:
3420 err = l2cap_move_channel_confirm(conn, cmd, cmd_len, data);
3421 break;
3422
3423 case L2CAP_MOVE_CHAN_CFM_RSP:
3424 err = l2cap_move_channel_confirm_rsp(conn, cmd, cmd_len, data);
3425 break;
3426
3300d9a9
CT		 3427 default:
3428 BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code);
3429 err = -EINVAL;
3430 break;
3431 }
3432
3433 return err;
3434}
3435
3436static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn,
3437 struct l2cap_cmd_hdr *cmd, u8 *data)
3438{
3439 switch (cmd->code) {
3440 case L2CAP_COMMAND_REJ:
3441 return 0;
3442
3443 case L2CAP_CONN_PARAM_UPDATE_REQ:
de73115a 3444 return l2cap_conn_param_update_req(conn, cmd, data);
3300d9a9
CT		 3445
3446 case L2CAP_CONN_PARAM_UPDATE_RSP:
3447 return 0;
3448
3449 default:
3450 BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code);
3451 return -EINVAL;
3452 }
3453}
3454
3455static inline void l2cap_sig_channel(struct l2cap_conn *conn,
3456 struct sk_buff *skb)
1da177e4
LT		 3457{
3458 u8 *data = skb->data;
3459 int len = skb->len;
3460 struct l2cap_cmd_hdr cmd;
3300d9a9 3461 int err;
1da177e4
LT		 3462
3463 l2cap_raw_recv(conn, skb);
3464
3465 while (len >= L2CAP_CMD_HDR_SIZE) {
88219a0f 3466 u16 cmd_len;
1da177e4
LT		 3467 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
3468 data += L2CAP_CMD_HDR_SIZE;
3469 len -= L2CAP_CMD_HDR_SIZE;
3470
88219a0f 3471 cmd_len = le16_to_cpu(cmd.len);
1da177e4 3472
88219a0f 3473 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len, cmd.ident);
1da177e4 3474
88219a0f 3475 if (cmd_len > len || !cmd.ident) {
1da177e4
LT		 3476 BT_DBG("corrupted command");
3477 break;
3478 }
3479
3300d9a9
CT		 3480 if (conn->hcon->type == LE_LINK)
3481 err = l2cap_le_sig_cmd(conn, &cmd, data);
3482 else
3483 err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data);
1da177e4
LT		 3484
3485 if (err) {
e2fd318e 3486 struct l2cap_cmd_rej_unk rej;
2c6d1a2e
GP		 3487
3488 BT_ERR("Wrong link type (%d)", err);
1da177e4
LT		 3489
3490 /* FIXME: Map err to a valid reason */
e2fd318e 3491 rej.reason = cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
1da177e4
LT		 3492 l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
3493 }
3494
88219a0f
AV		 3495 data += cmd_len;
3496 len -= cmd_len;
1da177e4
LT		 3497 }
3498
3499 kfree_skb(skb);
3500}
3501
47d1ec61 3502static int l2cap_check_fcs(struct l2cap_chan *chan, struct sk_buff *skb)
fcc203c3
GP		 3503{
3504 u16 our_fcs, rcv_fcs;
e4ca6d98
AE		 3505 int hdr_size;
3506
3507 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
3508 hdr_size = L2CAP_EXT_HDR_SIZE;
3509 else
3510 hdr_size = L2CAP_ENH_HDR_SIZE;
fcc203c3 3511
47d1ec61 3512 if (chan->fcs == L2CAP_FCS_CRC16) {
03a51213 3513 skb_trim(skb, skb->len - L2CAP_FCS_SIZE);
fcc203c3
GP		 3514 rcv_fcs = get_unaligned_le16(skb->data + skb->len);
3515 our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
3516
3517 if (our_fcs != rcv_fcs)
7a560e5c 3518 return -EBADMSG;
fcc203c3
GP		 3519 }
3520 return 0;
3521}
3522
525cd185 3523static inline void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan)
d5392c8f 3524{
88843ab0 3525 u32 control = 0;
d5392c8f 3526
6a026610 3527 chan->frames_sent = 0;
d5392c8f 3528
0b209fae 3529 control |= __set_reqseq(chan, chan->buffer_seq);
d5392c8f 3530
e2ab4353 3531 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
ab784b73 3532 control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR);
525cd185 3533 l2cap_send_sframe(chan, control);
e2ab4353 3534 set_bit(CONN_RNR_SENT, &chan->conn_state);
d5392c8f
GP		 3535 }
3536
e2ab4353 3537 if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
525cd185 3538 l2cap_retransmit_frames(chan);
d5392c8f 3539
525cd185 3540 l2cap_ertm_send(chan);
d5392c8f 3541
e2ab4353 3542 if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state) &&
6a026610 3543 chan->frames_sent == 0) {
ab784b73 3544 control |= __set_ctrl_super(chan, L2CAP_SUPER_RR);
525cd185 3545 l2cap_send_sframe(chan, control);
d5392c8f
GP		 3546 }
3547}
3548
fb45de7d 3549static int l2cap_add_to_srej_queue(struct l2cap_chan *chan, struct sk_buff *skb, u16 tx_seq, u8 sar)
8f17154f
GP		 3550{
3551 struct sk_buff *next_skb;
bfbacc11 3552 int tx_seq_offset, next_tx_seq_offset;
8f17154f
GP		 3553
3554 bt_cb(skb)->tx_seq = tx_seq;
3555 bt_cb(skb)->sar = sar;
3556
f1c6775b 3557 next_skb = skb_peek(&chan->srej_q);
8f17154f 3558
836be934 3559 tx_seq_offset = __seq_offset(chan, tx_seq, chan->buffer_seq);
bfbacc11 3560
039d9572 3561 while (next_skb) {
9b53350d
JPRV		 3562 if (bt_cb(next_skb)->tx_seq == tx_seq)
3563 return -EINVAL;
3564
836be934
AE		 3565 next_tx_seq_offset = __seq_offset(chan,
3566 bt_cb(next_skb)->tx_seq, chan->buffer_seq);
bfbacc11
JPRV		 3567
3568 if (next_tx_seq_offset > tx_seq_offset) {
f1c6775b 3569 __skb_queue_before(&chan->srej_q, next_skb, skb);
9b53350d 3570 return 0;
8f17154f
GP		 3571 }
3572
f1c6775b 3573 if (skb_queue_is_last(&chan->srej_q, next_skb))
039d9572
SJ		 3574 next_skb = NULL;
3575 else
3576 next_skb = skb_queue_next(&chan->srej_q, next_skb);
3577 }
8f17154f 3578
f1c6775b 3579 __skb_queue_tail(&chan->srej_q, skb);
9b53350d
JPRV		 3580
3581 return 0;
8f17154f
GP		 3582}
3583
84084a31
MM		 3584static void append_skb_frag(struct sk_buff *skb,
3585 struct sk_buff *new_frag, struct sk_buff **last_frag)
18778a63 3586{
84084a31
MM		 3587 /* skb->len reflects data in skb as well as all fragments
3588 * skb->data_len reflects only data in fragments
3589 */
3590 if (!skb_has_frag_list(skb))
3591 skb_shinfo(skb)->frag_list = new_frag;
3592
3593 new_frag->next = NULL;
3594
3595 (*last_frag)->next = new_frag;
3596 *last_frag = new_frag;
3597
3598 skb->len += new_frag->len;
3599 skb->data_len += new_frag->len;
3600 skb->truesize += new_frag->truesize;
3601}
3602
88843ab0 3603static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u32 control)
84084a31
MM		 3604{
3605 int err = -EINVAL;
18778a63 3606
7e0ef6ee
AE		 3607 switch (__get_ctrl_sar(chan, control)) {
3608 case L2CAP_SAR_UNSEGMENTED:
84084a31
MM		 3609 if (chan->sdu)
3610 break;
18778a63 3611
84084a31
MM		 3612 err = chan->ops->recv(chan->data, skb);
3613 break;
18778a63 3614
7e0ef6ee 3615 case L2CAP_SAR_START:
84084a31
MM		 3616 if (chan->sdu)
3617 break;
18778a63 3618
6f61fd47 3619 chan->sdu_len = get_unaligned_le16(skb->data);
03a51213 3620 skb_pull(skb, L2CAP_SDULEN_SIZE);
18778a63 3621
84084a31
MM		 3622 if (chan->sdu_len > chan->imtu) {
3623 err = -EMSGSIZE;
3624 break;
3625 }
1890d36b 3626
84084a31
MM		 3627 if (skb->len >= chan->sdu_len)
3628 break;
18778a63 3629
84084a31
MM		 3630 chan->sdu = skb;
3631 chan->sdu_last_frag = skb;
18778a63 3632
84084a31
MM		 3633 skb = NULL;
3634 err = 0;
18778a63
GP		 3635 break;
3636
7e0ef6ee 3637 case L2CAP_SAR_CONTINUE:
6f61fd47 3638 if (!chan->sdu)
84084a31 3639 break;
18778a63 3640
84084a31
MM		 3641 append_skb_frag(chan->sdu, skb,
3642 &chan->sdu_last_frag);
3643 skb = NULL;
18778a63 3644
84084a31
MM		 3645 if (chan->sdu->len >= chan->sdu_len)
3646 break;
4178ba46 3647
84084a31 3648 err = 0;
18778a63
GP		 3649 break;
3650
7e0ef6ee 3651 case L2CAP_SAR_END:
6f61fd47 3652 if (!chan->sdu)
84084a31 3653 break;
18778a63 3654
84084a31
MM		 3655 append_skb_frag(chan->sdu, skb,
3656 &chan->sdu_last_frag);
3657 skb = NULL;
4178ba46 3658
84084a31
MM		 3659 if (chan->sdu->len != chan->sdu_len)
3660 break;
18778a63 3661
84084a31 3662 err = chan->ops->recv(chan->data, chan->sdu);
1890d36b 3663
84084a31
MM		 3664 if (!err) {
3665 /* Reassembly complete */
3666 chan->sdu = NULL;
3667 chan->sdu_last_frag = NULL;
3668 chan->sdu_len = 0;
1890d36b 3669 }
18778a63
GP		 3670 break;
3671 }
3672
84084a31
MM		 3673 if (err) {
3674 kfree_skb(skb);
3675 kfree_skb(chan->sdu);
3676 chan->sdu = NULL;
3677 chan->sdu_last_frag = NULL;
3678 chan->sdu_len = 0;
3679 }
18778a63 3680
84084a31 3681 return err;
18778a63
GP		 3682}
3683
26f880d2 3684static void l2cap_ertm_enter_local_busy(struct l2cap_chan *chan)
712132eb 3685{
88843ab0 3686 u32 control;
712132eb 3687
26f880d2 3688 BT_DBG("chan %p, Enter local busy", chan);
712132eb 3689
26f880d2
MM		 3690 set_bit(CONN_LOCAL_BUSY, &chan->conn_state);
3691
0b209fae 3692 control = __set_reqseq(chan, chan->buffer_seq);
ab784b73 3693 control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR);
26f880d2
MM		 3694 l2cap_send_sframe(chan, control);
3695
3696 set_bit(CONN_RNR_SENT, &chan->conn_state);
3697
3698 __clear_ack_timer(chan);
3699}
3700
3701static void l2cap_ertm_exit_local_busy(struct l2cap_chan *chan)
3702{
88843ab0 3703 u32 control;
712132eb 3704
e2ab4353 3705 if (!test_bit(CONN_RNR_SENT, &chan->conn_state))
712132eb
GP		 3706 goto done;
3707
0b209fae 3708 control = __set_reqseq(chan, chan->buffer_seq);
e3781735 3709 control |= __set_ctrl_poll(chan);
ab784b73 3710 control |= __set_ctrl_super(chan, L2CAP_SUPER_RR);
525cd185 3711 l2cap_send_sframe(chan, control);
6a026610 3712 chan->retry_count = 1;
712132eb 3713
1a09bcb9
GP		 3714 __clear_retrans_timer(chan);
3715 __set_monitor_timer(chan);
712132eb 3716
e2ab4353 3717 set_bit(CONN_WAIT_F, &chan->conn_state);
712132eb
GP		 3718
3719done:
e2ab4353
GP		 3720 clear_bit(CONN_LOCAL_BUSY, &chan->conn_state);
3721 clear_bit(CONN_RNR_SENT, &chan->conn_state);
712132eb 3722
49208c9c 3723 BT_DBG("chan %p, Exit local busy", chan);
712132eb
GP		 3724}
3725
e328140f 3726void l2cap_chan_busy(struct l2cap_chan *chan, int busy)
1890d36b 3727{
e328140f
MM		 3728 if (chan->mode == L2CAP_MODE_ERTM) {
3729 if (busy)
3730 l2cap_ertm_enter_local_busy(chan);
3731 else
3732 l2cap_ertm_exit_local_busy(chan);
1890d36b 3733 }
1890d36b
GP		 3734}
3735
fb45de7d 3736static void l2cap_check_srej_gap(struct l2cap_chan *chan, u16 tx_seq)
8f17154f
GP		 3737{
3738 struct sk_buff *skb;
88843ab0 3739 u32 control;
8f17154f 3740
e328140f
MM		 3741 while ((skb = skb_peek(&chan->srej_q)) &&
3742 !test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
3743 int err;
3744
8f17154f
GP		 3745 if (bt_cb(skb)->tx_seq != tx_seq)
3746 break;
3747
f1c6775b 3748 skb = skb_dequeue(&chan->srej_q);
7e0ef6ee 3749 control = __set_ctrl_sar(chan, bt_cb(skb)->sar);
84084a31 3750 err = l2cap_reassemble_sdu(chan, skb, control);
e328140f
MM		 3751
3752 if (err < 0) {
3753 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3754 break;
3755 }
3756
836be934
AE		 3757 chan->buffer_seq_srej = __next_seq(chan, chan->buffer_seq_srej);
3758 tx_seq = __next_seq(chan, tx_seq);
8f17154f
GP		 3759 }
3760}
3761
fb45de7d 3762static void l2cap_resend_srejframe(struct l2cap_chan *chan, u16 tx_seq)
8f17154f 3763{
8f17154f 3764 struct srej_list *l, *tmp;
88843ab0 3765 u32 control;
8f17154f 3766
39d5a3ee 3767 list_for_each_entry_safe(l, tmp, &chan->srej_l, list) {
8f17154f
GP		 3768 if (l->tx_seq == tx_seq) {
3769 list_del(&l->list);
3770 kfree(l);
3771 return;
3772 }
ab784b73 3773 control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ);
0b209fae 3774 control |= __set_reqseq(chan, l->tx_seq);
525cd185 3775 l2cap_send_sframe(chan, control);
8f17154f 3776 list_del(&l->list);
39d5a3ee 3777 list_add_tail(&l->list, &chan->srej_l);
8f17154f
GP		 3778 }
3779}
3780
aef89f21 3781static int l2cap_send_srejframe(struct l2cap_chan *chan, u16 tx_seq)
8f17154f 3782{
8f17154f 3783 struct srej_list *new;
88843ab0 3784 u32 control;
8f17154f 3785
42e5c802 3786 while (tx_seq != chan->expected_tx_seq) {
ab784b73 3787 control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ);
0b209fae 3788 control |= __set_reqseq(chan, chan->expected_tx_seq);
525cd185 3789 l2cap_send_sframe(chan, control);
8f17154f
GP		 3790
3791 new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC);
aef89f21
SJ		 3792 if (!new)
3793 return -ENOMEM;
3794
42e5c802 3795 new->tx_seq = chan->expected_tx_seq;
836be934
AE		 3796
3797 chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq);
3798
39d5a3ee 3799 list_add_tail(&new->list, &chan->srej_l);
8f17154f 3800 }
836be934
AE		 3801
3802 chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq);
aef89f21
SJ		 3803
3804 return 0;
8f17154f
GP		 3805}
3806
88843ab0 3807static inline int l2cap_data_channel_iframe(struct l2cap_chan *chan, u32 rx_control, struct sk_buff *skb)
1c2acffb 3808{
fb45de7d 3809 u16 tx_seq = __get_txseq(chan, rx_control);
0b209fae 3810 u16 req_seq = __get_reqseq(chan, rx_control);
7e0ef6ee 3811 u8 sar = __get_ctrl_sar(chan, rx_control);
f6337c77 3812 int tx_seq_offset, expected_tx_seq_offset;
47d1ec61 3813 int num_to_ack = (chan->tx_win/6) + 1;
1c2acffb
GP		 3814 int err = 0;
3815
88843ab0 3816 BT_DBG("chan %p len %d tx_seq %d rx_control 0x%8.8x", chan, skb->len,
525cd185 3817 tx_seq, rx_control);
1c2acffb 3818
03f6715d 3819 if (__is_ctrl_final(chan, rx_control) &&
e2ab4353 3820 test_bit(CONN_WAIT_F, &chan->conn_state)) {
1a09bcb9 3821 __clear_monitor_timer(chan);
6a026610 3822 if (chan->unacked_frames > 0)
1a09bcb9 3823 __set_retrans_timer(chan);
e2ab4353 3824 clear_bit(CONN_WAIT_F, &chan->conn_state);
1d8f5d16
GP		 3825 }
3826
42e5c802
GP		 3827 chan->expected_ack_seq = req_seq;
3828 l2cap_drop_acked_frames(chan);
9f121a5a 3829
836be934 3830 tx_seq_offset = __seq_offset(chan, tx_seq, chan->buffer_seq);
9b53350d
JPRV		 3831
3832 /* invalid tx_seq */
47d1ec61 3833 if (tx_seq_offset >= chan->tx_win) {
8c1d787b 3834 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
9b53350d
JPRV		 3835 goto drop;
3836 }
3837
e2ab4353 3838 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state))
1890d36b
GP		 3839 goto drop;
3840
02f1b641
MM		 3841 if (tx_seq == chan->expected_tx_seq)
3842 goto expected;
3843
e2ab4353 3844 if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
8f17154f 3845 struct srej_list *first;
30afb5b2 3846
39d5a3ee 3847 first = list_first_entry(&chan->srej_l,
8f17154f
GP		 3848 struct srej_list, list);
3849 if (tx_seq == first->tx_seq) {
42e5c802 3850 l2cap_add_to_srej_queue(chan, skb, tx_seq, sar);
525cd185 3851 l2cap_check_srej_gap(chan, tx_seq);
8f17154f
GP		 3852
3853 list_del(&first->list);
3854 kfree(first);
3855
39d5a3ee 3856 if (list_empty(&chan->srej_l)) {
42e5c802 3857 chan->buffer_seq = chan->buffer_seq_srej;
e2ab4353 3858 clear_bit(CONN_SREJ_SENT, &chan->conn_state);
525cd185 3859 l2cap_send_ack(chan);
49208c9c 3860 BT_DBG("chan %p, Exit SREJ_SENT", chan);
8f17154f
GP		 3861 }
3862 } else {
3863 struct srej_list *l;
9b53350d
JPRV		 3864
3865 /* duplicated tx_seq */
42e5c802 3866 if (l2cap_add_to_srej_queue(chan, skb, tx_seq, sar) < 0)
9b53350d 3867 goto drop;
8f17154f 3868
39d5a3ee 3869 list_for_each_entry(l, &chan->srej_l, list) {
8f17154f 3870 if (l->tx_seq == tx_seq) {
525cd185 3871 l2cap_resend_srejframe(chan, tx_seq);
8f17154f
GP		 3872 return 0;
3873 }
3874 }
aef89f21
SJ		 3875
3876 err = l2cap_send_srejframe(chan, tx_seq);
3877 if (err < 0) {
3878 l2cap_send_disconn_req(chan->conn, chan, -err);
3879 return err;
3880 }
30afb5b2
GP		 3881 }
3882 } else {
836be934
AE		 3883 expected_tx_seq_offset = __seq_offset(chan,
3884 chan->expected_tx_seq, chan->buffer_seq);
9b53350d
JPRV		 3885
3886 /* duplicated tx_seq */
3887 if (tx_seq_offset < expected_tx_seq_offset)
3888 goto drop;
3889
e2ab4353 3890 set_bit(CONN_SREJ_SENT, &chan->conn_state);
1c2acffb 3891
49208c9c 3892 BT_DBG("chan %p, Enter SREJ", chan);
0e98958d 3893
39d5a3ee 3894 INIT_LIST_HEAD(&chan->srej_l);
42e5c802 3895 chan->buffer_seq_srej = chan->buffer_seq;
8f17154f 3896
f1c6775b 3897 __skb_queue_head_init(&chan->srej_q);
42e5c802 3898 l2cap_add_to_srej_queue(chan, skb, tx_seq, sar);
8f17154f 3899
e2ab4353 3900 set_bit(CONN_SEND_PBIT, &chan->conn_state);
ef54fd93 3901
aef89f21
SJ		 3902 err = l2cap_send_srejframe(chan, tx_seq);
3903 if (err < 0) {
3904 l2cap_send_disconn_req(chan->conn, chan, -err);
3905 return err;
3906 }
7fe9b298 3907
1a09bcb9 3908 __clear_ack_timer(chan);
1c2acffb 3909 }
30afb5b2
GP		 3910 return 0;
3911
8f17154f 3912expected:
836be934 3913 chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq);
8f17154f 3914
e2ab4353 3915 if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
3b1a9f3f
GP		 3916 bt_cb(skb)->tx_seq = tx_seq;
3917 bt_cb(skb)->sar = sar;
f1c6775b 3918 __skb_queue_tail(&chan->srej_q, skb);
8f17154f
GP		 3919 return 0;
3920 }
3921
84084a31 3922 err = l2cap_reassemble_sdu(chan, skb, rx_control);
836be934
AE		 3923 chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
3924
e328140f
MM		 3925 if (err < 0) {
3926 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3927 return err;
3928 }
2ece3684 3929
03f6715d 3930 if (__is_ctrl_final(chan, rx_control)) {
e2ab4353 3931 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
525cd185 3932 l2cap_retransmit_frames(chan);
4ec10d97
GP		 3933 }
3934
c1b4f43b 3935
6a026610
GP		 3936 chan->num_acked = (chan->num_acked + 1) % num_to_ack;
3937 if (chan->num_acked == num_to_ack - 1)
525cd185 3938 l2cap_send_ack(chan);
4d611e4d
GP		 3939 else
3940 __set_ack_timer(chan);
9e917af1 3941
8f17154f 3942 return 0;
9b53350d
JPRV		 3943
3944drop:
3945 kfree_skb(skb);
3946 return 0;
1c2acffb
GP		 3947}
3948
88843ab0 3949static inline void l2cap_data_channel_rrframe(struct l2cap_chan *chan, u32 rx_control)
1c2acffb 3950{
88843ab0 3951 BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan,
0b209fae 3952 __get_reqseq(chan, rx_control), rx_control);
0e98958d 3953
0b209fae 3954 chan->expected_ack_seq = __get_reqseq(chan, rx_control);
42e5c802 3955 l2cap_drop_acked_frames(chan);
1c2acffb 3956
e3781735 3957 if (__is_ctrl_poll(chan, rx_control)) {
e2ab4353
GP		 3958 set_bit(CONN_SEND_FBIT, &chan->conn_state);
3959 if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
3960 if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
6a026610 3961 (chan->unacked_frames > 0))
1a09bcb9 3962 __set_retrans_timer(chan);
05fbd89d 3963
e2ab4353 3964 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
525cd185 3965 l2cap_send_srejtail(chan);
05fbd89d 3966 } else {
525cd185 3967 l2cap_send_i_or_rr_or_rnr(chan);
05fbd89d 3968 }
1d8f5d16 3969
03f6715d 3970 } else if (__is_ctrl_final(chan, rx_control)) {
e2ab4353 3971 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
4ec10d97 3972
e2ab4353 3973 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
525cd185 3974 l2cap_retransmit_frames(chan);
2246b2f1 3975
e072745f 3976 } else {
e2ab4353 3977 if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
6a026610 3978 (chan->unacked_frames > 0))
1a09bcb9 3979 __set_retrans_timer(chan);
1c2acffb 3980
e2ab4353
GP		 3981 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3982 if (test_bit(CONN_SREJ_SENT, &chan->conn_state))
525cd185 3983 l2cap_send_ack(chan);
894718a6 3984 else
525cd185 3985 l2cap_ertm_send(chan);
e072745f
GP		 3986 }
3987}
2246b2f1 3988
88843ab0 3989static inline void l2cap_data_channel_rejframe(struct l2cap_chan *chan, u32 rx_control)
e072745f 3990{
0b209fae 3991 u16 tx_seq = __get_reqseq(chan, rx_control);
30afb5b2 3992
88843ab0 3993 BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control);
0e98958d 3994
e2ab4353 3995 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
e072745f 3996
42e5c802
GP		 3997 chan->expected_ack_seq = tx_seq;
3998 l2cap_drop_acked_frames(chan);
e072745f 3999
03f6715d 4000 if (__is_ctrl_final(chan, rx_control)) {
e2ab4353 4001 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
525cd185 4002 l2cap_retransmit_frames(chan);
e072745f 4003 } else {
525cd185 4004 l2cap_retransmit_frames(chan);
30afb5b2 4005
e2ab4353
GP		 4006 if (test_bit(CONN_WAIT_F, &chan->conn_state))
4007 set_bit(CONN_REJ_ACT, &chan->conn_state);
e072745f
GP		 4008 }
4009}
88843ab0 4010static inline void l2cap_data_channel_srejframe(struct l2cap_chan *chan, u32 rx_control)
e072745f 4011{
0b209fae 4012 u16 tx_seq = __get_reqseq(chan, rx_control);
30afb5b2 4013
88843ab0 4014 BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control);
0e98958d 4015
e2ab4353 4016 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
2246b2f1 4017
e3781735 4018 if (__is_ctrl_poll(chan, rx_control)) {
42e5c802
GP		 4019 chan->expected_ack_seq = tx_seq;
4020 l2cap_drop_acked_frames(chan);
3cb123d1 4021
e2ab4353 4022 set_bit(CONN_SEND_FBIT, &chan->conn_state);
525cd185 4023 l2cap_retransmit_one_frame(chan, tx_seq);
dfc909be 4024
525cd185 4025 l2cap_ertm_send(chan);
dfc909be 4026
e2ab4353 4027 if (test_bit(CONN_WAIT_F, &chan->conn_state)) {
6a026610 4028 chan->srej_save_reqseq = tx_seq;
e2ab4353 4029 set_bit(CONN_SREJ_ACT, &chan->conn_state);
ef54fd93 4030 }
03f6715d 4031 } else if (__is_ctrl_final(chan, rx_control)) {
e2ab4353 4032 if (test_bit(CONN_SREJ_ACT, &chan->conn_state) &&
6a026610 4033 chan->srej_save_reqseq == tx_seq)
e2ab4353 4034 clear_bit(CONN_SREJ_ACT, &chan->conn_state);
e072745f 4035 else
525cd185 4036 l2cap_retransmit_one_frame(chan, tx_seq);
e072745f 4037 } else {
525cd185 4038 l2cap_retransmit_one_frame(chan, tx_seq);
e2ab4353 4039 if (test_bit(CONN_WAIT_F, &chan->conn_state)) {
6a026610 4040 chan->srej_save_reqseq = tx_seq;
e2ab4353 4041 set_bit(CONN_SREJ_ACT, &chan->conn_state);
ef54fd93 4042 }
e072745f
GP		 4043 }
4044}
4045
88843ab0 4046static inline void l2cap_data_channel_rnrframe(struct l2cap_chan *chan, u32 rx_control)
e072745f 4047{
0b209fae 4048 u16 tx_seq = __get_reqseq(chan, rx_control);
e072745f 4049
88843ab0 4050 BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control);
0e98958d 4051
e2ab4353 4052 set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
42e5c802
GP		 4053 chan->expected_ack_seq = tx_seq;
4054 l2cap_drop_acked_frames(chan);
e072745f 4055
e3781735 4056 if (__is_ctrl_poll(chan, rx_control))
e2ab4353 4057 set_bit(CONN_SEND_FBIT, &chan->conn_state);
3cb123d1 4058
e2ab4353 4059 if (!test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
1a09bcb9 4060 __clear_retrans_timer(chan);
e3781735 4061 if (__is_ctrl_poll(chan, rx_control))
525cd185 4062 l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_FINAL);
99b0d4b7 4063 return;
e072745f 4064 }
99b0d4b7 4065
e3781735 4066 if (__is_ctrl_poll(chan, rx_control)) {
525cd185 4067 l2cap_send_srejtail(chan);
ab784b73
AE		 4068 } else {
4069 rx_control = __set_ctrl_super(chan, L2CAP_SUPER_RR);
4070 l2cap_send_sframe(chan, rx_control);
4071 }
e072745f
GP		 4072}
4073
88843ab0 4074static inline int l2cap_data_channel_sframe(struct l2cap_chan *chan, u32 rx_control, struct sk_buff *skb)
e072745f 4075{
88843ab0 4076 BT_DBG("chan %p rx_control 0x%8.8x len %d", chan, rx_control, skb->len);
e072745f 4077
03f6715d 4078 if (__is_ctrl_final(chan, rx_control) &&
e2ab4353 4079 test_bit(CONN_WAIT_F, &chan->conn_state)) {
1a09bcb9 4080 __clear_monitor_timer(chan);
6a026610 4081 if (chan->unacked_frames > 0)
1a09bcb9 4082 __set_retrans_timer(chan);
e2ab4353 4083 clear_bit(CONN_WAIT_F, &chan->conn_state);
e072745f
GP		 4084 }
4085
ab784b73
AE		 4086 switch (__get_ctrl_super(chan, rx_control)) {
4087 case L2CAP_SUPER_RR:
525cd185 4088 l2cap_data_channel_rrframe(chan, rx_control);
8f17154f
GP		 4089 break;
4090
ab784b73 4091 case L2CAP_SUPER_REJ:
525cd185 4092 l2cap_data_channel_rejframe(chan, rx_control);
e072745f 4093 break;
2246b2f1 4094
ab784b73 4095 case L2CAP_SUPER_SREJ:
525cd185 4096 l2cap_data_channel_srejframe(chan, rx_control);
e072745f
GP		 4097 break;
4098
ab784b73 4099 case L2CAP_SUPER_RNR:
525cd185 4100 l2cap_data_channel_rnrframe(chan, rx_control);
1c2acffb
GP		 4101 break;
4102 }
4103
faaebd19 4104 kfree_skb(skb);
1c2acffb
GP		 4105 return 0;
4106}
4107
218bb9df
GP		 4108static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb)
4109{
525cd185 4110 struct l2cap_chan *chan = l2cap_pi(sk)->chan;
88843ab0 4111 u32 control;
0b209fae 4112 u16 req_seq;
218bb9df
GP		 4113 int len, next_tx_seq_offset, req_seq_offset;
4114
88843ab0
AE		 4115 control = __get_control(chan, skb->data);
4116 skb_pull(skb, __ctrl_size(chan));
218bb9df
GP		 4117 len = skb->len;
4118
4119 /*
4120 * We can just drop the corrupted I-frame here.
4121 * Receiver will miss it and start proper recovery
4122 * procedures and ask retransmission.
4123 */
47d1ec61 4124 if (l2cap_check_fcs(chan, skb))
218bb9df
GP		 4125 goto drop;
4126
793c2f1c 4127 if (__is_sar_start(chan, control) && !__is_sframe(chan, control))
03a51213 4128 len -= L2CAP_SDULEN_SIZE;
218bb9df 4129
47d1ec61 4130 if (chan->fcs == L2CAP_FCS_CRC16)
03a51213 4131 len -= L2CAP_FCS_SIZE;
218bb9df 4132
47d1ec61 4133 if (len > chan->mps) {
8c1d787b 4134 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
218bb9df
GP		 4135 goto drop;
4136 }
4137
0b209fae 4138 req_seq = __get_reqseq(chan, control);
218bb9df 4139
836be934
AE		 4140 req_seq_offset = __seq_offset(chan, req_seq, chan->expected_ack_seq);
4141
4142 next_tx_seq_offset = __seq_offset(chan, chan->next_tx_seq,
4143 chan->expected_ack_seq);
218bb9df
GP		 4144
4145 /* check for invalid req-seq */
4146 if (req_seq_offset > next_tx_seq_offset) {
8c1d787b 4147 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
218bb9df
GP		 4148 goto drop;
4149 }
4150
793c2f1c 4151 if (!__is_sframe(chan, control)) {
218bb9df 4152 if (len < 0) {
8c1d787b 4153 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
218bb9df
GP		 4154 goto drop;
4155 }
4156
525cd185 4157 l2cap_data_channel_iframe(chan, control, skb);
218bb9df
GP		 4158 } else {
4159 if (len != 0) {
4160 BT_ERR("%d", len);
8c1d787b 4161 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
218bb9df
GP		 4162 goto drop;
4163 }
4164
525cd185 4165 l2cap_data_channel_sframe(chan, control, skb);
218bb9df
GP		 4166 }
4167
4168 return 0;
4169
4170drop:
4171 kfree_skb(skb);
4172 return 0;
4173}
4174
1da177e4
LT		 4175static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
4176{
48454079 4177 struct l2cap_chan *chan;
bf734843 4178 struct sock *sk = NULL;
88843ab0 4179 u32 control;
fb45de7d 4180 u16 tx_seq;
218bb9df 4181 int len;
1da177e4 4182
baa7e1fa 4183 chan = l2cap_get_chan_by_scid(conn, cid);
48454079 4184 if (!chan) {
1da177e4
LT		 4185 BT_DBG("unknown cid 0x%4.4x", cid);
4186 goto drop;
4187 }
4188
48454079 4189 sk = chan->sk;
6840ed07 4190
49208c9c 4191 BT_DBG("chan %p, len %d", chan, skb->len);
1da177e4 4192
89bc500e 4193 if (chan->state != BT_CONNECTED)
1da177e4
LT		 4194 goto drop;
4195
0c1bc5c6 4196 switch (chan->mode) {
1c2acffb
GP		 4197 case L2CAP_MODE_BASIC:
4198 /* If socket recv buffers overflows we drop data here
4199 * which is *bad* because L2CAP has to be reliable.
4200 * But we don't have any other choice. L2CAP doesn't
4201 * provide flow control mechanism. */
1da177e4 4202
0c1bc5c6 4203 if (chan->imtu < skb->len)
1c2acffb 4204 goto drop;
1da177e4 4205
23070494 4206 if (!chan->ops->recv(chan->data, skb))
1c2acffb
GP		 4207 goto done;
4208 break;
4209
4210 case L2CAP_MODE_ERTM:
218bb9df
GP		 4211 if (!sock_owned_by_user(sk)) {
4212 l2cap_ertm_data_rcv(sk, skb);
277ffbe3 4213 } else {
218bb9df 4214 if (sk_add_backlog(sk, skb))
277ffbe3 4215 goto drop;
277ffbe3 4216 }
1c2acffb 4217
fcafde2e 4218 goto done;
1c2acffb 4219
6840ed07 4220 case L2CAP_MODE_STREAMING:
88843ab0
AE		 4221 control = __get_control(chan, skb->data);
4222 skb_pull(skb, __ctrl_size(chan));
6840ed07
GP		 4223 len = skb->len;
4224
47d1ec61 4225 if (l2cap_check_fcs(chan, skb))
26000089
GP		 4226 goto drop;
4227
7e0ef6ee 4228 if (__is_sar_start(chan, control))
03a51213 4229 len -= L2CAP_SDULEN_SIZE;
6840ed07 4230
47d1ec61 4231 if (chan->fcs == L2CAP_FCS_CRC16)
03a51213 4232 len -= L2CAP_FCS_SIZE;
fcc203c3 4233
793c2f1c 4234 if (len > chan->mps || len < 0 || __is_sframe(chan, control))
6840ed07
GP		 4235 goto drop;
4236
fb45de7d 4237 tx_seq = __get_txseq(chan, control);
6840ed07 4238
84084a31
MM		 4239 if (chan->expected_tx_seq != tx_seq) {
4240 /* Frame(s) missing - must discard partial SDU */
4241 kfree_skb(chan->sdu);
4242 chan->sdu = NULL;
4243 chan->sdu_last_frag = NULL;
4244 chan->sdu_len = 0;
6840ed07 4245
84084a31
MM		 4246 /* TODO: Notify userland of missing data */
4247 }
4248
836be934 4249 chan->expected_tx_seq = __next_seq(chan, tx_seq);
84084a31
MM		 4250
4251 if (l2cap_reassemble_sdu(chan, skb, control) == -EMSGSIZE)
4252 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
6840ed07
GP		 4253
4254 goto done;
4255
1c2acffb 4256 default:
0c1bc5c6 4257 BT_DBG("chan %p: bad mode 0x%2.2x", chan, chan->mode);
1c2acffb
GP		 4258 break;
4259 }
1da177e4
LT		 4260
4261drop:
4262 kfree_skb(skb);
4263
4264done:
0139418c
MH		 4265 if (sk)
4266 bh_unlock_sock(sk);
4267
1da177e4
LT		 4268 return 0;
4269}
4270
8e036fc3 4271static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb)
1da177e4 4272{
6dcae1ea 4273 struct sock *sk = NULL;
23691d75 4274 struct l2cap_chan *chan;
1da177e4 4275
23691d75
GP		 4276 chan = l2cap_global_chan_by_psm(0, psm, conn->src);
4277 if (!chan)
1da177e4
LT		 4278 goto drop;
4279
23691d75
GP		 4280 sk = chan->sk;
4281
e0f0cb56
GP		 4282 bh_lock_sock(sk);
4283
1da177e4
LT		 4284 BT_DBG("sk %p, len %d", sk, skb->len);
4285
89bc500e 4286 if (chan->state != BT_BOUND && chan->state != BT_CONNECTED)
1da177e4
LT		 4287 goto drop;
4288
e13e21dc 4289 if (chan->imtu < skb->len)
1da177e4
LT		 4290 goto drop;
4291
23070494 4292 if (!chan->ops->recv(chan->data, skb))
1da177e4
LT		 4293 goto done;
4294
4295drop:
4296 kfree_skb(skb);
4297
4298done:
af05b30b
GP		 4299 if (sk)
4300 bh_unlock_sock(sk);
1da177e4
LT		 4301 return 0;
4302}
4303
9f69bda6
GP		 4304static inline int l2cap_att_channel(struct l2cap_conn *conn, __le16 cid, struct sk_buff *skb)
4305{
6dcae1ea 4306 struct sock *sk = NULL;
23691d75 4307 struct l2cap_chan *chan;
9f69bda6 4308
23691d75
GP		 4309 chan = l2cap_global_chan_by_scid(0, cid, conn->src);
4310 if (!chan)
9f69bda6
GP		 4311 goto drop;
4312
23691d75
GP		 4313 sk = chan->sk;
4314
9f69bda6
GP		 4315 bh_lock_sock(sk);
4316
4317 BT_DBG("sk %p, len %d", sk, skb->len);
4318
89bc500e 4319 if (chan->state != BT_BOUND && chan->state != BT_CONNECTED)
9f69bda6
GP		 4320 goto drop;
4321
e13e21dc 4322 if (chan->imtu < skb->len)
9f69bda6
GP		 4323 goto drop;
4324
23070494 4325 if (!chan->ops->recv(chan->data, skb))
9f69bda6
GP		 4326 goto done;
4327
4328drop:
4329 kfree_skb(skb);
4330
4331done:
4332 if (sk)
4333 bh_unlock_sock(sk);
4334 return 0;
4335}
4336
1da177e4
LT		 4337static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
4338{
4339 struct l2cap_hdr *lh = (void *) skb->data;
8e036fc3
AV		 4340 u16 cid, len;
4341 __le16 psm;
1da177e4
LT		 4342
4343 skb_pull(skb, L2CAP_HDR_SIZE);
4344 cid = __le16_to_cpu(lh->cid);
4345 len = __le16_to_cpu(lh->len);
4346
1c2acffb
GP		 4347 if (len != skb->len) {
4348 kfree_skb(skb);
4349 return;
4350 }
4351
1da177e4
LT		 4352 BT_DBG("len %d, cid 0x%4.4x", len, cid);
4353
4354 switch (cid) {
3300d9a9 4355 case L2CAP_CID_LE_SIGNALING:
8db4dc46 4356 case L2CAP_CID_SIGNALING:
1da177e4
LT		 4357 l2cap_sig_channel(conn, skb);
4358 break;
4359
8db4dc46 4360 case L2CAP_CID_CONN_LESS:
1b7bf4ed 4361 psm = get_unaligned_le16(skb->data);
1da177e4
LT		 4362 skb_pull(skb, 2);
4363 l2cap_conless_channel(conn, psm, skb);
4364 break;
4365
9f69bda6
GP		 4366 case L2CAP_CID_LE_DATA:
4367 l2cap_att_channel(conn, cid, skb);
4368 break;
4369
b501d6a1
AB		 4370 case L2CAP_CID_SMP:
4371 if (smp_sig_channel(conn, skb))
4372 l2cap_conn_del(conn->hcon, EACCES);
4373 break;
4374
1da177e4
LT		 4375 default:
4376 l2cap_data_channel(conn, cid, skb);
4377 break;
4378 }
4379}
4380
4381/* ---- L2CAP interface with lower layer (HCI) ---- */
4382
4383static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
4384{
4385 int exact = 0, lm1 = 0, lm2 = 0;
23691d75 4386 struct l2cap_chan *c;
1da177e4
LT		 4387
4388 if (type != ACL_LINK)
963cf687 4389 return -EINVAL;
1da177e4
LT		 4390
4391 BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
4392
4393 /* Find listening sockets and check their link_mode */
23691d75
GP		 4394 read_lock(&chan_list_lock);
4395 list_for_each_entry(c, &chan_list, global_l) {
4396 struct sock *sk = c->sk;
4343478f 4397
89bc500e 4398 if (c->state != BT_LISTEN)
1da177e4
LT		 4399 continue;
4400
4401 if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
2af6b9d5 4402 lm1 |= HCI_LM_ACCEPT;
43bd0f32 4403 if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
2af6b9d5 4404 lm1 |= HCI_LM_MASTER;
1da177e4 4405 exact++;
2af6b9d5
MH		 4406 } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) {
4407 lm2 |= HCI_LM_ACCEPT;
43bd0f32 4408 if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
2af6b9d5
MH		 4409 lm2 |= HCI_LM_MASTER;
4410 }
1da177e4 4411 }
23691d75 4412 read_unlock(&chan_list_lock);
1da177e4
LT		 4413
4414 return exact ? lm1 : lm2;
4415}
4416
4417static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
4418{
0139418c
MH		 4419 struct l2cap_conn *conn;
4420
1da177e4
LT		 4421 BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
4422
acd7d370 4423 if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK))
963cf687 4424 return -EINVAL;
1da177e4
LT		 4425
4426 if (!status) {
1da177e4
LT		 4427 conn = l2cap_conn_add(hcon, status);
4428 if (conn)
4429 l2cap_conn_ready(conn);
0139418c 4430 } else
e175072f 4431 l2cap_conn_del(hcon, bt_to_errno(status));
1da177e4
LT		 4432
4433 return 0;
4434}
4435
2950f21a
MH		 4436static int l2cap_disconn_ind(struct hci_conn *hcon)
4437{
4438 struct l2cap_conn *conn = hcon->l2cap_data;
4439
4440 BT_DBG("hcon %p", hcon);
4441
b5694506 4442 if ((hcon->type != ACL_LINK && hcon->type != LE_LINK) || !conn)
9f5a0d7b 4443 return HCI_ERROR_REMOTE_USER_TERM;
2950f21a
MH		 4444
4445 return conn->disc_reason;
4446}
4447
4448static int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
1da177e4
LT		 4449{
4450 BT_DBG("hcon %p reason %d", hcon, reason);
4451
acd7d370 4452 if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK))
963cf687 4453 return -EINVAL;
1da177e4 4454
e175072f 4455 l2cap_conn_del(hcon, bt_to_errno(reason));
0139418c 4456
1da177e4
LT		 4457 return 0;
4458}
4459
4343478f 4460static inline void l2cap_check_encryption(struct l2cap_chan *chan, u8 encrypt)
f62e4323 4461{
715ec005 4462 if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED)
255c7601
MH		 4463 return;
4464
f62e4323 4465 if (encrypt == 0x00) {
4343478f 4466 if (chan->sec_level == BT_SECURITY_MEDIUM) {
c9b66675 4467 __clear_chan_timer(chan);
f3f668b0 4468 __set_chan_timer(chan, L2CAP_ENC_TIMEOUT);
4343478f 4469 } else if (chan->sec_level == BT_SECURITY_HIGH)
0f852724 4470 l2cap_chan_close(chan, ECONNREFUSED);
f62e4323 4471 } else {
4343478f 4472 if (chan->sec_level == BT_SECURITY_MEDIUM)
c9b66675 4473 __clear_chan_timer(chan);
f62e4323
MH		 4474 }
4475}
4476
8c1b2355 4477static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
1da177e4 4478{
0139418c 4479 struct l2cap_conn *conn = hcon->l2cap_data;
48454079 4480 struct l2cap_chan *chan;
1da177e4 4481
0139418c 4482 if (!conn)
1da177e4 4483 return 0;
0139418c 4484
1da177e4
LT		 4485 BT_DBG("conn %p", conn);
4486
160dc6ac
VCG		 4487 if (hcon->type == LE_LINK) {
4488 smp_distribute_keys(conn, 0);
4489 del_timer(&conn->security_timer);
4490 }
4491
baa7e1fa 4492 read_lock(&conn->chan_lock);
1da177e4 4493
baa7e1fa 4494 list_for_each_entry(chan, &conn->chan_l, list) {
48454079 4495 struct sock *sk = chan->sk;
baa7e1fa 4496
1da177e4
LT		 4497 bh_lock_sock(sk);
4498
f1cb9af5
VCG		 4499 BT_DBG("chan->scid %d", chan->scid);
4500
4501 if (chan->scid == L2CAP_CID_LE_DATA) {
4502 if (!status && encrypt) {
4503 chan->sec_level = hcon->sec_level;
4504 l2cap_chan_ready(sk);
4505 }
4506
4507 bh_unlock_sock(sk);
4508 continue;
4509 }
4510
c1360a1c 4511 if (test_bit(CONF_CONNECT_PEND, &chan->conf_state)) {
6a8d3010
MH		 4512 bh_unlock_sock(sk);
4513 continue;
4514 }
4515
89bc500e
GP		 4516 if (!status && (chan->state == BT_CONNECTED ||
4517 chan->state == BT_CONFIG)) {
4343478f 4518 l2cap_check_encryption(chan, encrypt);
9719f8af
MH		 4519 bh_unlock_sock(sk);
4520 continue;
4521 }
4522
89bc500e 4523 if (chan->state == BT_CONNECT) {
b1235d79
MH		 4524 if (!status) {
4525 struct l2cap_conn_req req;
fe4128e0
GP		 4526 req.scid = cpu_to_le16(chan->scid);
4527 req.psm = chan->psm;
1da177e4 4528
fc7f8a7e 4529 chan->ident = l2cap_get_ident(conn);
c1360a1c 4530 set_bit(CONF_CONNECT_PEND, &chan->conf_state);
1da177e4 4531
fc7f8a7e 4532 l2cap_send_cmd(conn, chan->ident,
b1235d79
MH		 4533 L2CAP_CONN_REQ, sizeof(req), &req);
4534 } else {
c9b66675 4535 __clear_chan_timer(chan);
f3f668b0 4536 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
b1235d79 4537 }
89bc500e 4538 } else if (chan->state == BT_CONNECT2) {
b1235d79 4539 struct l2cap_conn_rsp rsp;
df3c3931 4540 __u16 res, stat;
1da177e4 4541
b1235d79 4542 if (!status) {
df3c3931
JH		 4543 if (bt_sk(sk)->defer_setup) {
4544 struct sock *parent = bt_sk(sk)->parent;
4545 res = L2CAP_CR_PEND;
4546 stat = L2CAP_CS_AUTHOR_PEND;
05e9a2f6
IK		 4547 if (parent)
4548 parent->sk_data_ready(parent, 0);
df3c3931 4549 } else {
05558911 4550 l2cap_state_change(chan, BT_CONFIG);
df3c3931
JH		 4551 res = L2CAP_CR_SUCCESS;
4552 stat = L2CAP_CS_NO_INFO;
4553 }
b1235d79 4554 } else {
89bc500e 4555 l2cap_state_change(chan, BT_DISCONN);
f3f668b0 4556 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
df3c3931
JH		 4557 res = L2CAP_CR_SEC_BLOCK;
4558 stat = L2CAP_CS_NO_INFO;
b1235d79
MH		 4559 }
4560
fe4128e0
GP		 4561 rsp.scid = cpu_to_le16(chan->dcid);
4562 rsp.dcid = cpu_to_le16(chan->scid);
df3c3931
JH		 4563 rsp.result = cpu_to_le16(res);
4564 rsp.status = cpu_to_le16(stat);
fc7f8a7e
GP		 4565 l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
4566 sizeof(rsp), &rsp);
b1235d79 4567 }
1da177e4
LT		 4568
4569 bh_unlock_sock(sk);
4570 }
4571
baa7e1fa 4572 read_unlock(&conn->chan_lock);
b1235d79 4573
1da177e4
LT		 4574 return 0;
4575}
4576
4577static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
4578{
4579 struct l2cap_conn *conn = hcon->l2cap_data;
4580
5a08ecce
AE		 4581 if (!conn)
4582 conn = l2cap_conn_add(hcon, 0);
4583
4584 if (!conn)
1da177e4
LT		 4585 goto drop;
4586
4587 BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
4588
e702112f 4589 if (!(flags & ACL_CONT)) {
1da177e4 4590 struct l2cap_hdr *hdr;
48454079 4591 struct l2cap_chan *chan;
89794813 4592 u16 cid;
1da177e4
LT		 4593 int len;
4594
4595 if (conn->rx_len) {
4596 BT_ERR("Unexpected start frame (len %d)", skb->len);
4597 kfree_skb(conn->rx_skb);
4598 conn->rx_skb = NULL;
4599 conn->rx_len = 0;
4600 l2cap_conn_unreliable(conn, ECOMM);
4601 }
4602
aae7fe22
AE		 4603 /* Start fragment always begin with Basic L2CAP header */
4604 if (skb->len < L2CAP_HDR_SIZE) {
1da177e4
LT		 4605 BT_ERR("Frame is too short (len %d)", skb->len);
4606 l2cap_conn_unreliable(conn, ECOMM);
4607 goto drop;
4608 }
4609
4610 hdr = (struct l2cap_hdr *) skb->data;
4611 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
89794813 4612 cid = __le16_to_cpu(hdr->cid);
1da177e4
LT		 4613
4614 if (len == skb->len) {
4615 /* Complete frame received */
4616 l2cap_recv_frame(conn, skb);
4617 return 0;
4618 }
4619
4620 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
4621
4622 if (skb->len > len) {
4623 BT_ERR("Frame is too long (len %d, expected len %d)",
4624 skb->len, len);
4625 l2cap_conn_unreliable(conn, ECOMM);
4626 goto drop;
4627 }
4628
baa7e1fa 4629 chan = l2cap_get_chan_by_scid(conn, cid);
89794813 4630
48454079
GP		 4631 if (chan && chan->sk) {
4632 struct sock *sk = chan->sk;
89794813 4633
0c1bc5c6 4634 if (chan->imtu < len - L2CAP_HDR_SIZE) {
48454079
GP		 4635 BT_ERR("Frame exceeding recv MTU (len %d, "
4636 "MTU %d)", len,
0c1bc5c6 4637 chan->imtu);
48454079
GP		 4638 bh_unlock_sock(sk);
4639 l2cap_conn_unreliable(conn, ECOMM);
4640 goto drop;
4641 }
89794813 4642 bh_unlock_sock(sk);
48454079 4643 }
89794813 4644
1da177e4 4645 /* Allocate skb for the complete frame (with header) */
af05b30b
GP		 4646 conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC);
4647 if (!conn->rx_skb)
1da177e4
LT		 4648 goto drop;
4649
d626f62b 4650 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
e1027a7c 4651 skb->len);
1da177e4
LT		 4652 conn->rx_len = len - skb->len;
4653 } else {
4654 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
4655
4656 if (!conn->rx_len) {
4657 BT_ERR("Unexpected continuation frame (len %d)", skb->len);
4658 l2cap_conn_unreliable(conn, ECOMM);
4659 goto drop;
4660 }
4661
4662 if (skb->len > conn->rx_len) {
4663 BT_ERR("Fragment is too long (len %d, expected %d)",
4664 skb->len, conn->rx_len);
4665 kfree_skb(conn->rx_skb);
4666 conn->rx_skb = NULL;
4667 conn->rx_len = 0;
4668 l2cap_conn_unreliable(conn, ECOMM);
4669 goto drop;
4670 }
4671
d626f62b 4672 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
e1027a7c 4673 skb->len);
1da177e4
LT		 4674 conn->rx_len -= skb->len;
4675
4676 if (!conn->rx_len) {
4677 /* Complete frame received */
4678 l2cap_recv_frame(conn, conn->rx_skb);
4679 conn->rx_skb = NULL;
4680 }
4681 }
4682
4683drop:
4684 kfree_skb(skb);
4685 return 0;
4686}
4687
aef7d97c 4688static int l2cap_debugfs_show(struct seq_file *f, void *p)
1da177e4 4689{
23691d75 4690 struct l2cap_chan *c;
1da177e4 4691
23691d75 4692 read_lock_bh(&chan_list_lock);
1da177e4 4693
23691d75
GP		 4694 list_for_each_entry(c, &chan_list, global_l) {
4695 struct sock *sk = c->sk;
101545f6 4696
903d343e 4697 seq_printf(f, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n",
aef7d97c
MH		 4698 batostr(&bt_sk(sk)->src),
4699 batostr(&bt_sk(sk)->dst),
89bc500e 4700 c->state, __le16_to_cpu(c->psm),
23691d75
GP		 4701 c->scid, c->dcid, c->imtu, c->omtu,
4702 c->sec_level, c->mode);
05558911 4703}
1da177e4 4704
23691d75 4705 read_unlock_bh(&chan_list_lock);
1da177e4 4706
aef7d97c 4707 return 0;
1da177e4
LT		 4708}
4709
aef7d97c
MH		 4710static int l2cap_debugfs_open(struct inode *inode, struct file *file)
4711{
4712 return single_open(file, l2cap_debugfs_show, inode->i_private);
4713}
4714
4715static const struct file_operations l2cap_debugfs_fops = {
4716 .open = l2cap_debugfs_open,
4717 .read = seq_read,
4718 .llseek = seq_lseek,
4719 .release = single_release,
4720};
4721
4722static struct dentry *l2cap_debugfs;
1da177e4 4723
1da177e4
LT		 4724static struct hci_proto l2cap_hci_proto = {
4725 .name = "L2CAP",
4726 .id = HCI_PROTO_L2CAP,
4727 .connect_ind = l2cap_connect_ind,
4728 .connect_cfm = l2cap_connect_cfm,
4729 .disconn_ind = l2cap_disconn_ind,
2950f21a 4730 .disconn_cfm = l2cap_disconn_cfm,
8c1b2355 4731 .security_cfm = l2cap_security_cfm,
1da177e4
LT		 4732 .recv_acldata = l2cap_recv_acldata
4733};
4734
64274518 4735int __init l2cap_init(void)
1da177e4
LT		 4736{
4737 int err;
be9d1227 4738
bb58f747 4739 err = l2cap_init_sockets();
1da177e4
LT		 4740 if (err < 0)
4741 return err;
4742
1da177e4
LT		 4743 err = hci_register_proto(&l2cap_hci_proto);
4744 if (err < 0) {
4745 BT_ERR("L2CAP protocol registration failed");
4746 bt_sock_unregister(BTPROTO_L2CAP);
4747 goto error;
4748 }
4749
aef7d97c
MH		 4750 if (bt_debugfs) {
4751 l2cap_debugfs = debugfs_create_file("l2cap", 0444,
4752 bt_debugfs, NULL, &l2cap_debugfs_fops);
4753 if (!l2cap_debugfs)
4754 BT_ERR("Failed to create L2CAP debug file");
4755 }
1da177e4 4756
1da177e4
LT		 4757 return 0;
4758
4759error:
bb58f747 4760 l2cap_cleanup_sockets();
1da177e4
LT		 4761 return err;
4762}
4763
64274518 4764void l2cap_exit(void)
1da177e4 4765{
aef7d97c 4766 debugfs_remove(l2cap_debugfs);
1da177e4 4767
1da177e4
LT		 4768 if (hci_unregister_proto(&l2cap_hci_proto) < 0)
4769 BT_ERR("L2CAP protocol unregistration failed");
4770
bb58f747 4771 l2cap_cleanup_sockets();
1da177e4
LT		 4772}
4773
d1c4a17d
GP		 4774module_param(disable_ertm, bool, 0644);
4775MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode");
kernel source for telekom puls (alcatel/mediatek)