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