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