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