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