projects / GitHub / mt8127 / android_kernel_alcatel_ttab.git / blame
? search:
summary | shortlog | log | commit | commitdiff | tree
blob | blame (incremental) | history | HEAD
Bluetooth: convert tx_task 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 117
d01b2ff4 118 mutex_lock(&conn->chan_lock);
baa7e1fa 119 c = __l2cap_get_chan_by_scid(conn, cid);
48454079 120 if (c)
aa2ac881 121 lock_sock(c->sk);
d01b2ff4 122 mutex_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 140
d01b2ff4 141 mutex_lock(&conn->chan_lock);
baa7e1fa 142 c = __l2cap_get_chan_by_ident(conn, ident);
48454079 143 if (c)
aa2ac881 144 lock_sock(c->sk);
d01b2ff4 145 mutex_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 383 /* Delete from channel list */
d01b2ff4 384 mutex_lock(&conn->chan_lock);
baa7e1fa 385 list_del(&chan->list);
d01b2ff4 386 mutex_unlock(&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
d01b2ff4 757 mutex_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 */
d01b2ff4 783 mutex_unlock(&conn->chan_lock);
89bc500e 784 l2cap_chan_close(chan, ECONNRESET);
d01b2ff4 785 utex_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
d01b2ff4 841 mutex_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
aa2ac881 892 lock_sock(parent);
62f3a2cf 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
d01b2ff4 906 mutex_lock(&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
d01b2ff4 922 mutex_unlock(&conn->chan_lock);
b62f328b
VT		 923
924clean:
aa2ac881 925 release_sock(parent);
b62f328b
VT		 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
d01b2ff4 957 mutex_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
d01b2ff4 979 mutex_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
d01b2ff4 989 mutex_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
d01b2ff4 998 mutex_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;
aa2ac881 1027 lock_sock(sk);
5d3de7df 1028 l2cap_chan_del(chan, err);
aa2ac881 1029 release_sock(sk);
5d3de7df
VCG		 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);
d01b2ff4 1089 mutex_init(&conn->chan_lock);
baa7e1fa
GP		 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{
d01b2ff4 1107 mutex_lock(&conn->chan_lock);
48454079 1108 __l2cap_chan_add(conn, chan);
d01b2ff4 1109 mutex_unlock(&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
d01b2ff4 1774 mutex_lock(&conn->chan_lock);
baa7e1fa 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 }
d01b2ff4 1790 mutex_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{
42e5c802 1963 chan->expected_ack_seq = 0;
6a026610 1964 chan->unacked_frames = 0;
42e5c802 1965 chan->buffer_seq = 0;
6a026610
GP		 1966 chan->num_acked = 0;
1967 chan->frames_sent = 0;
0565c1c2 1968
721c4181
GP		 1969 INIT_DELAYED_WORK(&chan->retrans_timer, l2cap_retrans_timeout);
1970 INIT_DELAYED_WORK(&chan->monitor_timer, l2cap_monitor_timeout);
1971 INIT_DELAYED_WORK(&chan->ack_timer, l2cap_ack_timeout);
0565c1c2 1972
f1c6775b 1973 skb_queue_head_init(&chan->srej_q);
1890d36b 1974
39d5a3ee 1975 INIT_LIST_HEAD(&chan->srej_l);
0565c1c2
GP		 1976}
1977
f2fcfcd6
GP		 1978static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask)
1979{
1980 switch (mode) {
1981 case L2CAP_MODE_STREAMING:
1982 case L2CAP_MODE_ERTM:
1983 if (l2cap_mode_supported(mode, remote_feat_mask))
1984 return mode;
1985 /* fall through */
1986 default:
1987 return L2CAP_MODE_BASIC;
1988 }
1989}
1990
6327eb98
AE		 1991static inline bool __l2cap_ews_supported(struct l2cap_chan *chan)
1992{
1993 return enable_hs && chan->conn->feat_mask & L2CAP_FEAT_EXT_WINDOW;
1994}
1995
f89cef09
AE		 1996static inline bool __l2cap_efs_supported(struct l2cap_chan *chan)
1997{
1998 return enable_hs && chan->conn->feat_mask & L2CAP_FEAT_EXT_FLOW;
1999}
2000
6327eb98
AE		 2001static inline void l2cap_txwin_setup(struct l2cap_chan *chan)
2002{
2003 if (chan->tx_win > L2CAP_DEFAULT_TX_WINDOW &&
836be934 2004 __l2cap_ews_supported(chan)) {
6327eb98
AE		 2005 /* use extended control field */
2006 set_bit(FLAG_EXT_CTRL, &chan->flags);
836be934
AE		 2007 chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
2008 } else {
6327eb98
AE		 2009 chan->tx_win = min_t(u16, chan->tx_win,
2010 L2CAP_DEFAULT_TX_WINDOW);
836be934
AE		 2011 chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW;
2012 }
6327eb98
AE		 2013}
2014
710f9b0a 2015static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data)
1da177e4 2016{
1da177e4 2017 struct l2cap_conf_req *req = data;
0c1bc5c6 2018 struct l2cap_conf_rfc rfc = { .mode = chan->mode };
1da177e4 2019 void *ptr = req->data;
c8f79162 2020 u16 size;
1da177e4 2021
49208c9c 2022 BT_DBG("chan %p", chan);
1da177e4 2023
73ffa904 2024 if (chan->num_conf_req || chan->num_conf_rsp)
f2fcfcd6
GP		 2025 goto done;
2026
0c1bc5c6 2027 switch (chan->mode) {
f2fcfcd6
GP		 2028 case L2CAP_MODE_STREAMING:
2029 case L2CAP_MODE_ERTM:
c1360a1c 2030 if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state))
85eb53c6 2031 break;
85eb53c6 2032
f89cef09
AE		 2033 if (__l2cap_efs_supported(chan))
2034 set_bit(FLAG_EFS_ENABLE, &chan->flags);
2035
2ba13ed6 2036 /* fall through */
f2fcfcd6 2037 default:
8c1d787b 2038 chan->mode = l2cap_select_mode(rfc.mode, chan->conn->feat_mask);
f2fcfcd6
GP		 2039 break;
2040 }
2041
2042done:
0c1bc5c6
GP		 2043 if (chan->imtu != L2CAP_DEFAULT_MTU)
2044 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu);
7990681c 2045
0c1bc5c6 2046 switch (chan->mode) {
65c7c491 2047 case L2CAP_MODE_BASIC:
8c1d787b
GP		 2048 if (!(chan->conn->feat_mask & L2CAP_FEAT_ERTM) &&
2049 !(chan->conn->feat_mask & L2CAP_FEAT_STREAMING))
63406504
GP		 2050 break;
2051
62547752
GP		 2052 rfc.mode = L2CAP_MODE_BASIC;
2053 rfc.txwin_size = 0;
2054 rfc.max_transmit = 0;
2055 rfc.retrans_timeout = 0;
2056 rfc.monitor_timeout = 0;
2057 rfc.max_pdu_size = 0;
2058
63406504
GP		 2059 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
2060 (unsigned long) &rfc);
65c7c491
MH		 2061 break;
2062
2063 case L2CAP_MODE_ERTM:
2064 rfc.mode = L2CAP_MODE_ERTM;
47d1ec61 2065 rfc.max_transmit = chan->max_tx;
f2fcfcd6
GP		 2066 rfc.retrans_timeout = 0;
2067 rfc.monitor_timeout = 0;
c8f79162
AE		 2068
2069 size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
2070 L2CAP_EXT_HDR_SIZE -
2071 L2CAP_SDULEN_SIZE -
2072 L2CAP_FCS_SIZE);
2073 rfc.max_pdu_size = cpu_to_le16(size);
f2fcfcd6 2074
6327eb98
AE		 2075 l2cap_txwin_setup(chan);
2076
2077 rfc.txwin_size = min_t(u16, chan->tx_win,
2078 L2CAP_DEFAULT_TX_WINDOW);
f2fcfcd6 2079
63406504
GP		 2080 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
2081 (unsigned long) &rfc);
2082
f89cef09
AE		 2083 if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
2084 l2cap_add_opt_efs(&ptr, chan);
2085
8c1d787b 2086 if (!(chan->conn->feat_mask & L2CAP_FEAT_FCS))
fcc203c3
GP		 2087 break;
2088
47d1ec61 2089 if (chan->fcs == L2CAP_FCS_NONE ||
c1360a1c 2090 test_bit(CONF_NO_FCS_RECV, &chan->conf_state)) {
47d1ec61
GP		 2091 chan->fcs = L2CAP_FCS_NONE;
2092 l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, chan->fcs);
fcc203c3 2093 }
6327eb98
AE		 2094
2095 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
2096 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
2097 chan->tx_win);
f2fcfcd6
GP		 2098 break;
2099
2100 case L2CAP_MODE_STREAMING:
2101 rfc.mode = L2CAP_MODE_STREAMING;
2102 rfc.txwin_size = 0;
2103 rfc.max_transmit = 0;
2104 rfc.retrans_timeout = 0;
2105 rfc.monitor_timeout = 0;
c8f79162
AE		 2106
2107 size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
2108 L2CAP_EXT_HDR_SIZE -
2109 L2CAP_SDULEN_SIZE -
2110 L2CAP_FCS_SIZE);
2111 rfc.max_pdu_size = cpu_to_le16(size);
65c7c491 2112
63406504
GP		 2113 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
2114 (unsigned long) &rfc);
2115
f89cef09
AE		 2116 if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
2117 l2cap_add_opt_efs(&ptr, chan);
2118
8c1d787b 2119 if (!(chan->conn->feat_mask & L2CAP_FEAT_FCS))
fcc203c3
GP		 2120 break;
2121
47d1ec61 2122 if (chan->fcs == L2CAP_FCS_NONE ||
c1360a1c 2123 test_bit(CONF_NO_FCS_RECV, &chan->conf_state)) {
47d1ec61
GP		 2124 chan->fcs = L2CAP_FCS_NONE;
2125 l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, chan->fcs);
fcc203c3 2126 }
65c7c491
MH		 2127 break;
2128 }
1da177e4 2129
fe4128e0 2130 req->dcid = cpu_to_le16(chan->dcid);
aca3192c 2131 req->flags = cpu_to_le16(0);
1da177e4
LT		 2132
2133 return ptr - data;
2134}
2135
73ffa904 2136static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data)
1da177e4 2137{
5dee9e7c
MH		 2138 struct l2cap_conf_rsp *rsp = data;
2139 void *ptr = rsp->data;
73ffa904
GP		 2140 void *req = chan->conf_req;
2141 int len = chan->conf_len;
5dee9e7c
MH		 2142 int type, hint, olen;
2143 unsigned long val;
6464f35f 2144 struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
42dceae2
AE		 2145 struct l2cap_conf_efs efs;
2146 u8 remote_efs = 0;
861d6882 2147 u16 mtu = L2CAP_DEFAULT_MTU;
5dee9e7c 2148 u16 result = L2CAP_CONF_SUCCESS;
c8f79162 2149 u16 size;
1da177e4 2150
73ffa904 2151 BT_DBG("chan %p", chan);
820ae1b8 2152
5dee9e7c
MH		 2153 while (len >= L2CAP_CONF_OPT_SIZE) {
2154 len -= l2cap_get_conf_opt(&req, &type, &olen, &val);
1da177e4 2155
589d2746 2156 hint = type & L2CAP_CONF_HINT;
47ec1dcd 2157 type &= L2CAP_CONF_MASK;
5dee9e7c
MH		 2158
2159 switch (type) {
2160 case L2CAP_CONF_MTU:
861d6882 2161 mtu = val;
5dee9e7c
MH		 2162 break;
2163
2164 case L2CAP_CONF_FLUSH_TO:
0c1bc5c6 2165 chan->flush_to = val;
5dee9e7c
MH		 2166 break;
2167
2168 case L2CAP_CONF_QOS:
2169 break;
2170
6464f35f
MH		 2171 case L2CAP_CONF_RFC:
2172 if (olen == sizeof(rfc))
2173 memcpy(&rfc, (void *) val, olen);
2174 break;
2175
fcc203c3
GP		 2176 case L2CAP_CONF_FCS:
2177 if (val == L2CAP_FCS_NONE)
c1360a1c 2178 set_bit(CONF_NO_FCS_RECV, &chan->conf_state);
42dceae2 2179 break;
fcc203c3 2180
42dceae2
AE		 2181 case L2CAP_CONF_EFS:
2182 remote_efs = 1;
2183 if (olen == sizeof(efs))
2184 memcpy(&efs, (void *) val, olen);
fcc203c3
GP		 2185 break;
2186
6327eb98
AE		 2187 case L2CAP_CONF_EWS:
2188 if (!enable_hs)
2189 return -ECONNREFUSED;
fcc203c3 2190
6327eb98
AE		 2191 set_bit(FLAG_EXT_CTRL, &chan->flags);
2192 set_bit(CONF_EWS_RECV, &chan->conf_state);
836be934 2193 chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
6327eb98 2194 chan->remote_tx_win = val;
fcc203c3
GP		 2195 break;
2196
5dee9e7c
MH		 2197 default:
2198 if (hint)
2199 break;
2200
2201 result = L2CAP_CONF_UNKNOWN;
2202 *((u8 *) ptr++) = type;
2203 break;
2204 }
2205 }
2206
73ffa904 2207 if (chan->num_conf_rsp || chan->num_conf_req > 1)
f2fcfcd6
GP		 2208 goto done;
2209
0c1bc5c6 2210 switch (chan->mode) {
f2fcfcd6
GP		 2211 case L2CAP_MODE_STREAMING:
2212 case L2CAP_MODE_ERTM:
c1360a1c 2213 if (!test_bit(CONF_STATE2_DEVICE, &chan->conf_state)) {
0c1bc5c6 2214 chan->mode = l2cap_select_mode(rfc.mode,
8c1d787b 2215 chan->conn->feat_mask);
85eb53c6
GP		 2216 break;
2217 }
2218
42dceae2
AE		 2219 if (remote_efs) {
2220 if (__l2cap_efs_supported(chan))
2221 set_bit(FLAG_EFS_ENABLE, &chan->flags);
2222 else
2223 return -ECONNREFUSED;
2224 }
2225
0c1bc5c6 2226 if (chan->mode != rfc.mode)
f2fcfcd6 2227 return -ECONNREFUSED;
742e519b 2228
f2fcfcd6 2229 break;
f2fcfcd6
GP		 2230 }
2231
2232done:
0c1bc5c6 2233 if (chan->mode != rfc.mode) {
f2fcfcd6 2234 result = L2CAP_CONF_UNACCEPT;
0c1bc5c6 2235 rfc.mode = chan->mode;
f2fcfcd6 2236
73ffa904 2237 if (chan->num_conf_rsp == 1)
f2fcfcd6
GP		 2238 return -ECONNREFUSED;
2239
2240 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2241 sizeof(rfc), (unsigned long) &rfc);
2242 }
2243
5dee9e7c
MH		 2244 if (result == L2CAP_CONF_SUCCESS) {
2245 /* Configure output options and let the other side know
2246 * which ones we don't like. */
2247
f2fcfcd6
GP		 2248 if (mtu < L2CAP_DEFAULT_MIN_MTU)
2249 result = L2CAP_CONF_UNACCEPT;
2250 else {
0c1bc5c6 2251 chan->omtu = mtu;
c1360a1c 2252 set_bit(CONF_MTU_DONE, &chan->conf_state);
f2fcfcd6 2253 }
0c1bc5c6 2254 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->omtu);
6464f35f 2255
42dceae2
AE		 2256 if (remote_efs) {
2257 if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
2258 efs.stype != L2CAP_SERV_NOTRAFIC &&
2259 efs.stype != chan->local_stype) {
2260
2261 result = L2CAP_CONF_UNACCEPT;
2262
2263 if (chan->num_conf_req >= 1)
2264 return -ECONNREFUSED;
2265
2266 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
3e6b3b95 2267 sizeof(efs),
42dceae2 2268 (unsigned long) &efs);
0e8b207e 2269 } else {
3e6b3b95 2270 /* Send PENDING Conf Rsp */
0e8b207e
AE		 2271 result = L2CAP_CONF_PENDING;
2272 set_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
42dceae2
AE		 2273 }
2274 }
2275
f2fcfcd6
GP		 2276 switch (rfc.mode) {
2277 case L2CAP_MODE_BASIC:
47d1ec61 2278 chan->fcs = L2CAP_FCS_NONE;
c1360a1c 2279 set_bit(CONF_MODE_DONE, &chan->conf_state);
f2fcfcd6
GP		 2280 break;
2281
2282 case L2CAP_MODE_ERTM:
6327eb98
AE		 2283 if (!test_bit(CONF_EWS_RECV, &chan->conf_state))
2284 chan->remote_tx_win = rfc.txwin_size;
2285 else
2286 rfc.txwin_size = L2CAP_DEFAULT_TX_WINDOW;
86b1b263 2287
2c03a7a4 2288 chan->remote_max_tx = rfc.max_transmit;
1c762159 2289
c8f79162
AE		 2290 size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
2291 chan->conn->mtu -
2292 L2CAP_EXT_HDR_SIZE -
2293 L2CAP_SDULEN_SIZE -
2294 L2CAP_FCS_SIZE);
2295 rfc.max_pdu_size = cpu_to_le16(size);
2296 chan->remote_mps = size;
f2fcfcd6 2297
10467e9e
GP		 2298 rfc.retrans_timeout =
2299 le16_to_cpu(L2CAP_DEFAULT_RETRANS_TO);
2300 rfc.monitor_timeout =
2301 le16_to_cpu(L2CAP_DEFAULT_MONITOR_TO);
f2fcfcd6 2302
c1360a1c 2303 set_bit(CONF_MODE_DONE, &chan->conf_state);
68ae6639
GP		 2304
2305 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2306 sizeof(rfc), (unsigned long) &rfc);
2307
42dceae2
AE		 2308 if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
2309 chan->remote_id = efs.id;
2310 chan->remote_stype = efs.stype;
2311 chan->remote_msdu = le16_to_cpu(efs.msdu);
2312 chan->remote_flush_to =
2313 le32_to_cpu(efs.flush_to);
2314 chan->remote_acc_lat =
2315 le32_to_cpu(efs.acc_lat);
2316 chan->remote_sdu_itime =
2317 le32_to_cpu(efs.sdu_itime);
2318 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
2319 sizeof(efs), (unsigned long) &efs);
2320 }
f2fcfcd6
GP		 2321 break;
2322
2323 case L2CAP_MODE_STREAMING:
c8f79162
AE		 2324 size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
2325 chan->conn->mtu -
2326 L2CAP_EXT_HDR_SIZE -
2327 L2CAP_SDULEN_SIZE -
2328 L2CAP_FCS_SIZE);
2329 rfc.max_pdu_size = cpu_to_le16(size);
2330 chan->remote_mps = size;
f2fcfcd6 2331
c1360a1c 2332 set_bit(CONF_MODE_DONE, &chan->conf_state);
68ae6639
GP		 2333
2334 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2335 sizeof(rfc), (unsigned long) &rfc);
2336
f2fcfcd6
GP		 2337 break;
2338
2339 default:
5dee9e7c 2340 result = L2CAP_CONF_UNACCEPT;
5dee9e7c 2341
6464f35f 2342 memset(&rfc, 0, sizeof(rfc));
0c1bc5c6 2343 rfc.mode = chan->mode;
f2fcfcd6 2344 }
6464f35f 2345
f2fcfcd6 2346 if (result == L2CAP_CONF_SUCCESS)
c1360a1c 2347 set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
f2fcfcd6 2348 }
fe4128e0 2349 rsp->scid = cpu_to_le16(chan->dcid);
5dee9e7c
MH		 2350 rsp->result = cpu_to_le16(result);
2351 rsp->flags = cpu_to_le16(0x0000);
2352
2353 return ptr - data;
1da177e4
LT		 2354}
2355
b4450035 2356static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, void *data, u16 *result)
f2fcfcd6 2357{
f2fcfcd6
GP		 2358 struct l2cap_conf_req *req = data;
2359 void *ptr = req->data;
2360 int type, olen;
2361 unsigned long val;
2362 struct l2cap_conf_rfc rfc;
66af7aaf 2363 struct l2cap_conf_efs efs;
f2fcfcd6 2364
fe4128e0 2365 BT_DBG("chan %p, rsp %p, len %d, req %p", chan, rsp, len, data);
f2fcfcd6
GP		 2366
2367 while (len >= L2CAP_CONF_OPT_SIZE) {
2368 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
2369
2370 switch (type) {
2371 case L2CAP_CONF_MTU:
2372 if (val < L2CAP_DEFAULT_MIN_MTU) {
2373 *result = L2CAP_CONF_UNACCEPT;
0c1bc5c6 2374 chan->imtu = L2CAP_DEFAULT_MIN_MTU;
f2fcfcd6 2375 } else
0c1bc5c6
GP		 2376 chan->imtu = val;
2377 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu);
f2fcfcd6
GP		 2378 break;
2379
2380 case L2CAP_CONF_FLUSH_TO:
0c1bc5c6 2381 chan->flush_to = val;
f2fcfcd6 2382 l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
0c1bc5c6 2383 2, chan->flush_to);
f2fcfcd6
GP		 2384 break;
2385
2386 case L2CAP_CONF_RFC:
2387 if (olen == sizeof(rfc))
2388 memcpy(&rfc, (void *)val, olen);
2389
c1360a1c 2390 if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state) &&
0c1bc5c6 2391 rfc.mode != chan->mode)
f2fcfcd6
GP		 2392 return -ECONNREFUSED;
2393
47d1ec61 2394 chan->fcs = 0;
f2fcfcd6
GP		 2395
2396 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2397 sizeof(rfc), (unsigned long) &rfc);
2398 break;
6327eb98
AE		 2399
2400 case L2CAP_CONF_EWS:
2401 chan->tx_win = min_t(u16, val,
2402 L2CAP_DEFAULT_EXT_WINDOW);
3e6b3b95
GP		 2403 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
2404 chan->tx_win);
6327eb98 2405 break;
66af7aaf
AE		 2406
2407 case L2CAP_CONF_EFS:
2408 if (olen == sizeof(efs))
2409 memcpy(&efs, (void *)val, olen);
2410
2411 if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
2412 efs.stype != L2CAP_SERV_NOTRAFIC &&
2413 efs.stype != chan->local_stype)
2414 return -ECONNREFUSED;
2415
2416 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
2417 sizeof(efs), (unsigned long) &efs);
2418 break;
f2fcfcd6
GP		 2419 }
2420 }
2421
0c1bc5c6 2422 if (chan->mode == L2CAP_MODE_BASIC && chan->mode != rfc.mode)
6c2ea7a8
GP		 2423 return -ECONNREFUSED;
2424
0c1bc5c6 2425 chan->mode = rfc.mode;
6c2ea7a8 2426
0e8b207e 2427 if (*result == L2CAP_CONF_SUCCESS || *result == L2CAP_CONF_PENDING) {
f2fcfcd6
GP		 2428 switch (rfc.mode) {
2429 case L2CAP_MODE_ERTM:
47d1ec61
GP		 2430 chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
2431 chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
2432 chan->mps = le16_to_cpu(rfc.max_pdu_size);
66af7aaf
AE		 2433
2434 if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
2435 chan->local_msdu = le16_to_cpu(efs.msdu);
2436 chan->local_sdu_itime =
2437 le32_to_cpu(efs.sdu_itime);
2438 chan->local_acc_lat = le32_to_cpu(efs.acc_lat);
2439 chan->local_flush_to =
2440 le32_to_cpu(efs.flush_to);
2441 }
f2fcfcd6 2442 break;
66af7aaf 2443
f2fcfcd6 2444 case L2CAP_MODE_STREAMING:
47d1ec61 2445 chan->mps = le16_to_cpu(rfc.max_pdu_size);
f2fcfcd6
GP		 2446 }
2447 }
2448
fe4128e0 2449 req->dcid = cpu_to_le16(chan->dcid);
f2fcfcd6
GP		 2450 req->flags = cpu_to_le16(0x0000);
2451
2452 return ptr - data;
2453}
2454
fe4128e0 2455static int l2cap_build_conf_rsp(struct l2cap_chan *chan, void *data, u16 result, u16 flags)
1da177e4
LT		 2456{
2457 struct l2cap_conf_rsp *rsp = data;
2458 void *ptr = rsp->data;
1da177e4 2459
fe4128e0 2460 BT_DBG("chan %p", chan);
1da177e4 2461
fe4128e0 2462 rsp->scid = cpu_to_le16(chan->dcid);
5dee9e7c 2463 rsp->result = cpu_to_le16(result);
aca3192c 2464 rsp->flags = cpu_to_le16(flags);
1da177e4
LT		 2465
2466 return ptr - data;
2467}
2468
8c1d787b 2469void __l2cap_connect_rsp_defer(struct l2cap_chan *chan)
710f9b0a
GP		 2470{
2471 struct l2cap_conn_rsp rsp;
8c1d787b 2472 struct l2cap_conn *conn = chan->conn;
710f9b0a
GP		 2473 u8 buf[128];
2474
fe4128e0
GP		 2475 rsp.scid = cpu_to_le16(chan->dcid);
2476 rsp.dcid = cpu_to_le16(chan->scid);
710f9b0a
GP		 2477 rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
2478 rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
2479 l2cap_send_cmd(conn, chan->ident,
2480 L2CAP_CONN_RSP, sizeof(rsp), &rsp);
2481
c1360a1c 2482 if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
710f9b0a
GP		 2483 return;
2484
710f9b0a
GP		 2485 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2486 l2cap_build_conf_req(chan, buf), buf);
2487 chan->num_conf_req++;
2488}
2489
47d1ec61 2490static void l2cap_conf_rfc_get(struct l2cap_chan *chan, void *rsp, int len)
7b1c0049 2491{
7b1c0049
GP		 2492 int type, olen;
2493 unsigned long val;
2494 struct l2cap_conf_rfc rfc;
2495
47d1ec61 2496 BT_DBG("chan %p, rsp %p, len %d", chan, rsp, len);
7b1c0049 2497
0c1bc5c6 2498 if ((chan->mode != L2CAP_MODE_ERTM) && (chan->mode != L2CAP_MODE_STREAMING))
7b1c0049
GP		 2499 return;
2500
2501 while (len >= L2CAP_CONF_OPT_SIZE) {
2502 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
2503
2504 switch (type) {
2505 case L2CAP_CONF_RFC:
2506 if (olen == sizeof(rfc))
2507 memcpy(&rfc, (void *)val, olen);
2508 goto done;
2509 }
2510 }
2511
2512done:
2513 switch (rfc.mode) {
2514 case L2CAP_MODE_ERTM:
47d1ec61
GP		 2515 chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
2516 chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
2517 chan->mps = le16_to_cpu(rfc.max_pdu_size);
7b1c0049
GP		 2518 break;
2519 case L2CAP_MODE_STREAMING:
47d1ec61 2520 chan->mps = le16_to_cpu(rfc.max_pdu_size);
7b1c0049
GP		 2521 }
2522}
2523
4e8402a3
MH		 2524static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2525{
e2fd318e 2526 struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data;
4e8402a3 2527
e2fd318e 2528 if (rej->reason != L2CAP_REJ_NOT_UNDERSTOOD)
4e8402a3
MH		 2529 return 0;
2530
2531 if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
2532 cmd->ident == conn->info_ident) {
4e8402a3 2533 del_timer(&conn->info_timer);
984947dc
MH		 2534
2535 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
e1027a7c 2536 conn->info_ident = 0;
984947dc 2537
4e8402a3
MH		 2538 l2cap_conn_start(conn);
2539 }
2540
2541 return 0;
2542}
2543
1da177e4
LT		 2544static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2545{
1da177e4
LT		 2546 struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
2547 struct l2cap_conn_rsp rsp;
23691d75 2548 struct l2cap_chan *chan = NULL, *pchan;
d793fe8c 2549 struct sock *parent, *sk = NULL;
e7c29cb1 2550 int result, status = L2CAP_CS_NO_INFO;
1da177e4
LT		 2551
2552 u16 dcid = 0, scid = __le16_to_cpu(req->scid);
e7c29cb1 2553 __le16 psm = req->psm;
1da177e4
LT		 2554
2555 BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid);
2556
2557 /* Check if we have socket listening on psm */
23691d75
GP		 2558 pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, conn->src);
2559 if (!pchan) {
1da177e4
LT		 2560 result = L2CAP_CR_BAD_PSM;
2561 goto sendresp;
2562 }
2563
23691d75
GP		 2564 parent = pchan->sk;
2565
aa2ac881 2566 lock_sock(parent);
e0f0cb56 2567
e7c29cb1
MH		 2568 /* Check if the ACL is secure enough (if not SDP) */
2569 if (psm != cpu_to_le16(0x0001) &&
2570 !hci_conn_check_link_mode(conn->hcon)) {
9f5a0d7b 2571 conn->disc_reason = HCI_ERROR_AUTH_FAILURE;
e7c29cb1
MH		 2572 result = L2CAP_CR_SEC_BLOCK;
2573 goto response;
2574 }
2575
1da177e4
LT		 2576 result = L2CAP_CR_NO_MEM;
2577
2578 /* Check for backlog size */
2579 if (sk_acceptq_is_full(parent)) {
8e87d142 2580 BT_DBG("backlog full %d", parent->sk_ack_backlog);
1da177e4
LT		 2581 goto response;
2582 }
2583
80808e43
GP		 2584 chan = pchan->ops->new_connection(pchan->data);
2585 if (!chan)
1da177e4
LT		 2586 goto response;
2587
80808e43
GP		 2588 sk = chan->sk;
2589
d01b2ff4 2590 mutex_lock(&conn->chan_lock);
1da177e4
LT		 2591
2592 /* Check if we already have channel with that dcid */
baa7e1fa 2593 if (__l2cap_get_chan_by_dcid(conn, scid)) {
d01b2ff4 2594 mutex_unlock(&conn->chan_lock);
1da177e4 2595 sock_set_flag(sk, SOCK_ZAPPED);
ba3bd0ee 2596 chan->ops->close(chan->data);
1da177e4
LT		 2597 goto response;
2598 }
2599
2600 hci_conn_hold(conn->hcon);
2601
1da177e4
LT		 2602 bacpy(&bt_sk(sk)->src, conn->src);
2603 bacpy(&bt_sk(sk)->dst, conn->dst);
fe4128e0
GP		 2604 chan->psm = psm;
2605 chan->dcid = scid;
1da177e4 2606
d1010240
GP		 2607 bt_accept_enqueue(parent, sk);
2608
48454079
GP		 2609 __l2cap_chan_add(conn, chan);
2610
fe4128e0 2611 dcid = chan->scid;
1da177e4 2612
c9b66675 2613 __set_chan_timer(chan, sk->sk_sndtimeo);
1da177e4 2614
fc7f8a7e 2615 chan->ident = cmd->ident;
1da177e4 2616
984947dc 2617 if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
d45fc423 2618 if (l2cap_chan_check_security(chan)) {
f66dc81f 2619 if (bt_sk(sk)->defer_setup) {
89bc500e 2620 l2cap_state_change(chan, BT_CONNECT2);
f66dc81f
MH		 2621 result = L2CAP_CR_PEND;
2622 status = L2CAP_CS_AUTHOR_PEND;
2623 parent->sk_data_ready(parent, 0);
2624 } else {
89bc500e 2625 l2cap_state_change(chan, BT_CONFIG);
f66dc81f
MH		 2626 result = L2CAP_CR_SUCCESS;
2627 status = L2CAP_CS_NO_INFO;
2628 }
79d554a6 2629 } else {
89bc500e 2630 l2cap_state_change(chan, BT_CONNECT2);
79d554a6
MH		 2631 result = L2CAP_CR_PEND;
2632 status = L2CAP_CS_AUTHEN_PEND;
2633 }
2634 } else {
89bc500e 2635 l2cap_state_change(chan, BT_CONNECT2);
79d554a6
MH		 2636 result = L2CAP_CR_PEND;
2637 status = L2CAP_CS_NO_INFO;
1da177e4
LT		 2638 }
2639
d01b2ff4 2640 mutex_unlock(&conn->chan_lock);
1da177e4
LT		 2641
2642response:
aa2ac881 2643 release_sock(parent);
1da177e4
LT		 2644
2645sendresp:
aca3192c
YH		 2646 rsp.scid = cpu_to_le16(scid);
2647 rsp.dcid = cpu_to_le16(dcid);
2648 rsp.result = cpu_to_le16(result);
2649 rsp.status = cpu_to_le16(status);
1da177e4 2650 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
79d554a6
MH		 2651
2652 if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
2653 struct l2cap_info_req info;
2654 info.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
2655
2656 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
2657 conn->info_ident = l2cap_get_ident(conn);
2658
2659 mod_timer(&conn->info_timer, jiffies +
2660 msecs_to_jiffies(L2CAP_INFO_TIMEOUT));
2661
2662 l2cap_send_cmd(conn, conn->info_ident,
2663 L2CAP_INFO_REQ, sizeof(info), &info);
2664 }
2665
c1360a1c 2666 if (chan && !test_bit(CONF_REQ_SENT, &chan->conf_state) &&
e9aeb2dd
GP		 2667 result == L2CAP_CR_SUCCESS) {
2668 u8 buf[128];
c1360a1c 2669 set_bit(CONF_REQ_SENT, &chan->conf_state);
e9aeb2dd 2670 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
73ffa904
GP		 2671 l2cap_build_conf_req(chan, buf), buf);
2672 chan->num_conf_req++;
e9aeb2dd
GP		 2673 }
2674
1da177e4
LT		 2675 return 0;
2676}
2677
2678static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2679{
2680 struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
2681 u16 scid, dcid, result, status;
48454079 2682 struct l2cap_chan *chan;
1da177e4
LT		 2683 struct sock *sk;
2684 u8 req[128];
2685
2686 scid = __le16_to_cpu(rsp->scid);
2687 dcid = __le16_to_cpu(rsp->dcid);
2688 result = __le16_to_cpu(rsp->result);
2689 status = __le16_to_cpu(rsp->status);
2690
2691 BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", dcid, scid, result, status);
2692
2693 if (scid) {
baa7e1fa 2694 chan = l2cap_get_chan_by_scid(conn, scid);
48454079 2695 if (!chan)
57d3b22b 2696 return -EFAULT;
1da177e4 2697 } else {
baa7e1fa 2698 chan = l2cap_get_chan_by_ident(conn, cmd->ident);
48454079 2699 if (!chan)
57d3b22b 2700 return -EFAULT;
1da177e4
LT		 2701 }
2702
48454079
GP		 2703 sk = chan->sk;
2704
1da177e4
LT		 2705 switch (result) {
2706 case L2CAP_CR_SUCCESS:
89bc500e 2707 l2cap_state_change(chan, BT_CONFIG);
fc7f8a7e 2708 chan->ident = 0;
fe4128e0 2709 chan->dcid = dcid;
c1360a1c 2710 clear_bit(CONF_CONNECT_PEND, &chan->conf_state);
6a8d3010 2711
c1360a1c 2712 if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
e9aeb2dd
GP		 2713 break;
2714
1da177e4 2715 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
73ffa904
GP		 2716 l2cap_build_conf_req(chan, req), req);
2717 chan->num_conf_req++;
1da177e4
LT		 2718 break;
2719
2720 case L2CAP_CR_PEND:
c1360a1c 2721 set_bit(CONF_CONNECT_PEND, &chan->conf_state);
1da177e4
LT		 2722 break;
2723
2724 default:
48454079 2725 l2cap_chan_del(chan, ECONNREFUSED);
1da177e4
LT		 2726 break;
2727 }
2728
aa2ac881 2729 release_sock(sk);
1da177e4
LT		 2730 return 0;
2731}
2732
47d1ec61 2733static inline void set_default_fcs(struct l2cap_chan *chan)
8c462b60
MM		 2734{
2735 /* FCS is enabled only in ERTM or streaming mode, if one or both
2736 * sides request it.
2737 */
0c1bc5c6 2738 if (chan->mode != L2CAP_MODE_ERTM && chan->mode != L2CAP_MODE_STREAMING)
47d1ec61 2739 chan->fcs = L2CAP_FCS_NONE;
c1360a1c 2740 else if (!test_bit(CONF_NO_FCS_RECV, &chan->conf_state))
47d1ec61 2741 chan->fcs = L2CAP_FCS_CRC16;
8c462b60
MM		 2742}
2743
88219a0f 2744static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
1da177e4
LT		 2745{
2746 struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
2747 u16 dcid, flags;
2748 u8 rsp[64];
48454079 2749 struct l2cap_chan *chan;
1da177e4 2750 struct sock *sk;
5dee9e7c 2751 int len;
1da177e4
LT		 2752
2753 dcid = __le16_to_cpu(req->dcid);
2754 flags = __le16_to_cpu(req->flags);
2755
2756 BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
2757
baa7e1fa 2758 chan = l2cap_get_chan_by_scid(conn, dcid);
48454079 2759 if (!chan)
1da177e4
LT		 2760 return -ENOENT;
2761
48454079
GP		 2762 sk = chan->sk;
2763
033b1142 2764 if (chan->state != BT_CONFIG && chan->state != BT_CONNECT2) {
e2fd318e
IK		 2765 struct l2cap_cmd_rej_cid rej;
2766
2767 rej.reason = cpu_to_le16(L2CAP_REJ_INVALID_CID);
2768 rej.scid = cpu_to_le16(chan->scid);
2769 rej.dcid = cpu_to_le16(chan->dcid);
df6bd743 2770
df6bd743
GP		 2771 l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ,
2772 sizeof(rej), &rej);
354f60a9 2773 goto unlock;
df6bd743 2774 }
354f60a9 2775
5dee9e7c 2776 /* Reject if config buffer is too small. */
88219a0f 2777 len = cmd_len - sizeof(*req);
7ac28817 2778 if (len < 0 || chan->conf_len + len > sizeof(chan->conf_req)) {
5dee9e7c 2779 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
fe4128e0 2780 l2cap_build_conf_rsp(chan, rsp,
5dee9e7c
MH		 2781 L2CAP_CONF_REJECT, flags), rsp);
2782 goto unlock;
2783 }
2784
2785 /* Store config. */
73ffa904
GP		 2786 memcpy(chan->conf_req + chan->conf_len, req->data, len);
2787 chan->conf_len += len;
1da177e4
LT		 2788
2789 if (flags & 0x0001) {
2790 /* Incomplete config. Send empty response. */
2791 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
fe4128e0 2792 l2cap_build_conf_rsp(chan, rsp,
5dee9e7c 2793 L2CAP_CONF_SUCCESS, 0x0001), rsp);
1da177e4
LT		 2794 goto unlock;
2795 }
2796
2797 /* Complete config. */
73ffa904 2798 len = l2cap_parse_conf_req(chan, rsp);
f2fcfcd6 2799 if (len < 0) {
e92c8e70 2800 l2cap_send_disconn_req(conn, chan, ECONNRESET);
1da177e4 2801 goto unlock;
f2fcfcd6 2802 }
1da177e4 2803
5dee9e7c 2804 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
73ffa904 2805 chan->num_conf_rsp++;
5dee9e7c 2806
5dee9e7c 2807 /* Reset config buffer. */
73ffa904 2808 chan->conf_len = 0;
5dee9e7c 2809
c1360a1c 2810 if (!test_bit(CONF_OUTPUT_DONE, &chan->conf_state))
876d9484
MH		 2811 goto unlock;
2812
c1360a1c 2813 if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
47d1ec61 2814 set_default_fcs(chan);
fcc203c3 2815
89bc500e 2816 l2cap_state_change(chan, BT_CONNECTED);
e90bac06 2817
42e5c802
GP		 2818 chan->next_tx_seq = 0;
2819 chan->expected_tx_seq = 0;
58d35f87 2820 skb_queue_head_init(&chan->tx_q);
0c1bc5c6 2821 if (chan->mode == L2CAP_MODE_ERTM)
525cd185 2822 l2cap_ertm_init(chan);
0565c1c2 2823
1da177e4 2824 l2cap_chan_ready(sk);
876d9484
MH		 2825 goto unlock;
2826 }
2827
c1360a1c 2828 if (!test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) {
79d554a6 2829 u8 buf[64];
1da177e4 2830 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
73ffa904
GP		 2831 l2cap_build_conf_req(chan, buf), buf);
2832 chan->num_conf_req++;
1da177e4
LT		 2833 }
2834
0e8b207e
AE		 2835 /* Got Conf Rsp PENDING from remote side and asume we sent
2836 Conf Rsp PENDING in the code above */
2837 if (test_bit(CONF_REM_CONF_PEND, &chan->conf_state) &&
2838 test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
2839
2840 /* check compatibility */
2841
2842 clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
2843 set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
2844
2845 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
3e6b3b95 2846 l2cap_build_conf_rsp(chan, rsp,
0e8b207e
AE		 2847 L2CAP_CONF_SUCCESS, 0x0000), rsp);
2848 }
2849
1da177e4 2850unlock:
aa2ac881 2851 release_sock(sk);
1da177e4
LT		 2852 return 0;
2853}
2854
2855static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2856{
2857 struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
2858 u16 scid, flags, result;
48454079 2859 struct l2cap_chan *chan;
1da177e4 2860 struct sock *sk;
7b1c0049 2861 int len = cmd->len - sizeof(*rsp);
1da177e4
LT		 2862
2863 scid = __le16_to_cpu(rsp->scid);
2864 flags = __le16_to_cpu(rsp->flags);
2865 result = __le16_to_cpu(rsp->result);
2866
af05b30b
GP		 2867 BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x",
2868 scid, flags, result);
1da177e4 2869
baa7e1fa 2870 chan = l2cap_get_chan_by_scid(conn, scid);
48454079 2871 if (!chan)
1da177e4
LT		 2872 return 0;
2873
48454079
GP		 2874 sk = chan->sk;
2875
1da177e4
LT		 2876 switch (result) {
2877 case L2CAP_CONF_SUCCESS:
47d1ec61 2878 l2cap_conf_rfc_get(chan, rsp->data, len);
0e8b207e 2879 clear_bit(CONF_REM_CONF_PEND, &chan->conf_state);
1da177e4
LT		 2880 break;
2881
0e8b207e
AE		 2882 case L2CAP_CONF_PENDING:
2883 set_bit(CONF_REM_CONF_PEND, &chan->conf_state);
2884
2885 if (test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
2886 char buf[64];
2887
2888 len = l2cap_parse_conf_rsp(chan, rsp->data, len,
2889 buf, &result);
2890 if (len < 0) {
2891 l2cap_send_disconn_req(conn, chan, ECONNRESET);
2892 goto done;
2893 }
2894
2895 /* check compatibility */
2896
2897 clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
2898 set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
2899
2900 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
3e6b3b95 2901 l2cap_build_conf_rsp(chan, buf,
0e8b207e
AE		 2902 L2CAP_CONF_SUCCESS, 0x0000), buf);
2903 }
2904 goto done;
2905
1da177e4 2906 case L2CAP_CONF_UNACCEPT:
73ffa904 2907 if (chan->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
f2fcfcd6
GP		 2908 char req[64];
2909
c2c77ec8 2910 if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) {
e92c8e70 2911 l2cap_send_disconn_req(conn, chan, ECONNRESET);
c2c77ec8
AE		 2912 goto done;
2913 }
2914
f2fcfcd6
GP		 2915 /* throw out any old stored conf requests */
2916 result = L2CAP_CONF_SUCCESS;
b4450035
GP		 2917 len = l2cap_parse_conf_rsp(chan, rsp->data, len,
2918 req, &result);
f2fcfcd6 2919 if (len < 0) {
e92c8e70 2920 l2cap_send_disconn_req(conn, chan, ECONNRESET);
f2fcfcd6
GP		 2921 goto done;
2922 }
2923
2924 l2cap_send_cmd(conn, l2cap_get_ident(conn),
2925 L2CAP_CONF_REQ, len, req);
73ffa904 2926 chan->num_conf_req++;
f2fcfcd6
GP		 2927 if (result != L2CAP_CONF_SUCCESS)
2928 goto done;
2929 break;
1da177e4
LT		 2930 }
2931
8e87d142 2932 default:
b1235d79 2933 sk->sk_err = ECONNRESET;
f3f668b0 2934 __set_chan_timer(chan, L2CAP_DISC_REJ_TIMEOUT);
e92c8e70 2935 l2cap_send_disconn_req(conn, chan, ECONNRESET);
1da177e4
LT		 2936 goto done;
2937 }
2938
2939 if (flags & 0x01)
2940 goto done;
2941
c1360a1c 2942 set_bit(CONF_INPUT_DONE, &chan->conf_state);
1da177e4 2943
c1360a1c 2944 if (test_bit(CONF_OUTPUT_DONE, &chan->conf_state)) {
47d1ec61 2945 set_default_fcs(chan);
fcc203c3 2946
89bc500e 2947 l2cap_state_change(chan, BT_CONNECTED);
42e5c802
GP		 2948 chan->next_tx_seq = 0;
2949 chan->expected_tx_seq = 0;
58d35f87 2950 skb_queue_head_init(&chan->tx_q);
0c1bc5c6 2951 if (chan->mode == L2CAP_MODE_ERTM)
525cd185 2952 l2cap_ertm_init(chan);
0565c1c2 2953
1da177e4
LT		 2954 l2cap_chan_ready(sk);
2955 }
2956
2957done:
aa2ac881 2958 release_sock(sk);
1da177e4
LT		 2959 return 0;
2960}
2961
2962static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2963{
2964 struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
2965 struct l2cap_disconn_rsp rsp;
2966 u16 dcid, scid;
48454079 2967 struct l2cap_chan *chan;
1da177e4
LT		 2968 struct sock *sk;
2969
2970 scid = __le16_to_cpu(req->scid);
2971 dcid = __le16_to_cpu(req->dcid);
2972
2973 BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
2974
baa7e1fa 2975 chan = l2cap_get_chan_by_scid(conn, dcid);
48454079 2976 if (!chan)
1da177e4
LT		 2977 return 0;
2978
48454079
GP		 2979 sk = chan->sk;
2980
fe4128e0
GP		 2981 rsp.dcid = cpu_to_le16(chan->scid);
2982 rsp.scid = cpu_to_le16(chan->dcid);
1da177e4
LT		 2983 l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
2984
2985 sk->sk_shutdown = SHUTDOWN_MASK;
2986
48454079 2987 l2cap_chan_del(chan, ECONNRESET);
aa2ac881 2988 release_sock(sk);
1da177e4 2989
ba3bd0ee 2990 chan->ops->close(chan->data);
1da177e4
LT		 2991 return 0;
2992}
2993
2994static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2995{
2996 struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
2997 u16 dcid, scid;
48454079 2998 struct l2cap_chan *chan;
1da177e4
LT		 2999 struct sock *sk;
3000
3001 scid = __le16_to_cpu(rsp->scid);
3002 dcid = __le16_to_cpu(rsp->dcid);
3003
3004 BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
3005
baa7e1fa 3006 chan = l2cap_get_chan_by_scid(conn, scid);
48454079 3007 if (!chan)
1da177e4
LT		 3008 return 0;
3009
48454079
GP		 3010 sk = chan->sk;
3011
48454079 3012 l2cap_chan_del(chan, 0);
aa2ac881 3013 release_sock(sk);
1da177e4 3014
ba3bd0ee 3015 chan->ops->close(chan->data);
1da177e4
LT		 3016 return 0;
3017}
3018
3019static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3020{
3021 struct l2cap_info_req *req = (struct l2cap_info_req *) data;
1da177e4
LT		 3022 u16 type;
3023
3024 type = __le16_to_cpu(req->type);
3025
3026 BT_DBG("type 0x%4.4x", type);
3027
f0709e03
MH		 3028 if (type == L2CAP_IT_FEAT_MASK) {
3029 u8 buf[8];
44dd46de 3030 u32 feat_mask = l2cap_feat_mask;
f0709e03
MH		 3031 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
3032 rsp->type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
3033 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
d1c4a17d 3034 if (!disable_ertm)
fcc203c3
GP		 3035 feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
3036 | L2CAP_FEAT_FCS;
a5fd6f30 3037 if (enable_hs)
6327eb98
AE		 3038 feat_mask |= L2CAP_FEAT_EXT_FLOW
3039 | L2CAP_FEAT_EXT_WINDOW;
a5fd6f30 3040
1b7bf4ed 3041 put_unaligned_le32(feat_mask, rsp->data);
f0709e03
MH		 3042 l2cap_send_cmd(conn, cmd->ident,
3043 L2CAP_INFO_RSP, sizeof(buf), buf);
e1027a7c
MH		 3044 } else if (type == L2CAP_IT_FIXED_CHAN) {
3045 u8 buf[12];
3046 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
50a147cd
MM		 3047
3048 if (enable_hs)
3049 l2cap_fixed_chan[0] |= L2CAP_FC_A2MP;
3050 else
3051 l2cap_fixed_chan[0] &= ~L2CAP_FC_A2MP;
3052
e1027a7c
MH		 3053 rsp->type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
3054 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
c6337ea6 3055 memcpy(rsp->data, l2cap_fixed_chan, sizeof(l2cap_fixed_chan));
e1027a7c
MH		 3056 l2cap_send_cmd(conn, cmd->ident,
3057 L2CAP_INFO_RSP, sizeof(buf), buf);
f0709e03
MH		 3058 } else {
3059 struct l2cap_info_rsp rsp;
3060 rsp.type = cpu_to_le16(type);
3061 rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP);
3062 l2cap_send_cmd(conn, cmd->ident,
3063 L2CAP_INFO_RSP, sizeof(rsp), &rsp);
3064 }
1da177e4
LT		 3065
3066 return 0;
3067}
3068
3069static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3070{
3071 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
3072 u16 type, result;
3073
3074 type = __le16_to_cpu(rsp->type);
3075 result = __le16_to_cpu(rsp->result);
3076
3077 BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
3078
e90165be
AE		 3079 /* L2CAP Info req/rsp are unbound to channels, add extra checks */
3080 if (cmd->ident != conn->info_ident ||
3081 conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)
3082 return 0;
3083
4e8402a3
MH		 3084 del_timer(&conn->info_timer);
3085
adb08ede
VT		 3086 if (result != L2CAP_IR_SUCCESS) {
3087 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3088 conn->info_ident = 0;
3089
3090 l2cap_conn_start(conn);
3091
3092 return 0;
3093 }
3094
984947dc 3095 if (type == L2CAP_IT_FEAT_MASK) {
83985319 3096 conn->feat_mask = get_unaligned_le32(rsp->data);
4e8402a3 3097
47ec1dcd 3098 if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
e1027a7c
MH		 3099 struct l2cap_info_req req;
3100 req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
3101
3102 conn->info_ident = l2cap_get_ident(conn);
3103
3104 l2cap_send_cmd(conn, conn->info_ident,
3105 L2CAP_INFO_REQ, sizeof(req), &req);
3106 } else {
3107 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3108 conn->info_ident = 0;
3109
3110 l2cap_conn_start(conn);
3111 }
3112 } else if (type == L2CAP_IT_FIXED_CHAN) {
984947dc 3113 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
e1027a7c 3114 conn->info_ident = 0;
984947dc
MH		 3115
3116 l2cap_conn_start(conn);
3117 }
4e8402a3 3118
1da177e4
LT		 3119 return 0;
3120}
3121
f94ff6ff
MM		 3122static inline int l2cap_create_channel_req(struct l2cap_conn *conn,
3123 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
3124 void *data)
3125{
3126 struct l2cap_create_chan_req *req = data;
3127 struct l2cap_create_chan_rsp rsp;
3128 u16 psm, scid;
3129
3130 if (cmd_len != sizeof(*req))
3131 return -EPROTO;
3132
3133 if (!enable_hs)
3134 return -EINVAL;
3135
3136 psm = le16_to_cpu(req->psm);
3137 scid = le16_to_cpu(req->scid);
3138
3139 BT_DBG("psm %d, scid %d, amp_id %d", psm, scid, req->amp_id);
3140
3141 /* Placeholder: Always reject */
3142 rsp.dcid = 0;
3143 rsp.scid = cpu_to_le16(scid);
3144 rsp.result = L2CAP_CR_NO_MEM;
3145 rsp.status = L2CAP_CS_NO_INFO;
3146
3147 l2cap_send_cmd(conn, cmd->ident, L2CAP_CREATE_CHAN_RSP,
3148 sizeof(rsp), &rsp);
3149
3150 return 0;
3151}
3152
3153static inline int l2cap_create_channel_rsp(struct l2cap_conn *conn,
3154 struct l2cap_cmd_hdr *cmd, void *data)
3155{
3156 BT_DBG("conn %p", conn);
3157
3158 return l2cap_connect_rsp(conn, cmd, data);
3159}
3160
8d5a04a1
MM		 3161static void l2cap_send_move_chan_rsp(struct l2cap_conn *conn, u8 ident,
3162 u16 icid, u16 result)
3163{
3164 struct l2cap_move_chan_rsp rsp;
3165
3166 BT_DBG("icid %d, result %d", icid, result);
3167
3168 rsp.icid = cpu_to_le16(icid);
3169 rsp.result = cpu_to_le16(result);
3170
3171 l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_RSP, sizeof(rsp), &rsp);
3172}
3173
3174static void l2cap_send_move_chan_cfm(struct l2cap_conn *conn,
3175 struct l2cap_chan *chan, u16 icid, u16 result)
3176{
3177 struct l2cap_move_chan_cfm cfm;
3178 u8 ident;
3179
3180 BT_DBG("icid %d, result %d", icid, result);
3181
3182 ident = l2cap_get_ident(conn);
3183 if (chan)
3184 chan->ident = ident;
3185
3186 cfm.icid = cpu_to_le16(icid);
3187 cfm.result = cpu_to_le16(result);
3188
3189 l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM, sizeof(cfm), &cfm);
3190}
3191
3192static void l2cap_send_move_chan_cfm_rsp(struct l2cap_conn *conn, u8 ident,
3193 u16 icid)
3194{
3195 struct l2cap_move_chan_cfm_rsp rsp;
3196
3197 BT_DBG("icid %d", icid);
3198
3199 rsp.icid = cpu_to_le16(icid);
3200 l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM_RSP, sizeof(rsp), &rsp);
3201}
3202
3203static inline int l2cap_move_channel_req(struct l2cap_conn *conn,
3204 struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data)
3205{
3206 struct l2cap_move_chan_req *req = data;
3207 u16 icid = 0;
3208 u16 result = L2CAP_MR_NOT_ALLOWED;
3209
3210 if (cmd_len != sizeof(*req))
3211 return -EPROTO;
3212
3213 icid = le16_to_cpu(req->icid);
3214
3215 BT_DBG("icid %d, dest_amp_id %d", icid, req->dest_amp_id);
3216
3217 if (!enable_hs)
3218 return -EINVAL;
3219
3220 /* Placeholder: Always refuse */
3221 l2cap_send_move_chan_rsp(conn, cmd->ident, icid, result);
3222
3223 return 0;
3224}
3225
3226static inline int l2cap_move_channel_rsp(struct l2cap_conn *conn,
3227 struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data)
3228{
3229 struct l2cap_move_chan_rsp *rsp = data;
3230 u16 icid, result;
3231
3232 if (cmd_len != sizeof(*rsp))
3233 return -EPROTO;
3234
3235 icid = le16_to_cpu(rsp->icid);
3236 result = le16_to_cpu(rsp->result);
3237
3238 BT_DBG("icid %d, result %d", icid, result);
3239
3240 /* Placeholder: Always unconfirmed */
3241 l2cap_send_move_chan_cfm(conn, NULL, icid, L2CAP_MC_UNCONFIRMED);
3242
3243 return 0;
3244}
3245
3246static inline int l2cap_move_channel_confirm(struct l2cap_conn *conn,
3247 struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data)
3248{
3249 struct l2cap_move_chan_cfm *cfm = data;
3250 u16 icid, result;
3251
3252 if (cmd_len != sizeof(*cfm))
3253 return -EPROTO;
3254
3255 icid = le16_to_cpu(cfm->icid);
3256 result = le16_to_cpu(cfm->result);
3257
3258 BT_DBG("icid %d, result %d", icid, result);
3259
3260 l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid);
3261
3262 return 0;
3263}
3264
3265static inline int l2cap_move_channel_confirm_rsp(struct l2cap_conn *conn,
3266 struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data)
3267{
3268 struct l2cap_move_chan_cfm_rsp *rsp = data;
3269 u16 icid;
3270
3271 if (cmd_len != sizeof(*rsp))
3272 return -EPROTO;
3273
3274 icid = le16_to_cpu(rsp->icid);
3275
3276 BT_DBG("icid %d", icid);
3277
3278 return 0;
3279}
3280
e2174ca4 3281static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency,
de73115a
CT		 3282 u16 to_multiplier)
3283{
3284 u16 max_latency;
3285
3286 if (min > max || min < 6 || max > 3200)
3287 return -EINVAL;
3288
3289 if (to_multiplier < 10 || to_multiplier > 3200)
3290 return -EINVAL;
3291
3292 if (max >= to_multiplier * 8)
3293 return -EINVAL;
3294
3295 max_latency = (to_multiplier * 8 / max) - 1;
3296 if (latency > 499 || latency > max_latency)
3297 return -EINVAL;
3298
3299 return 0;
3300}
3301
3302static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
3303 struct l2cap_cmd_hdr *cmd, u8 *data)
3304{
3305 struct hci_conn *hcon = conn->hcon;
3306 struct l2cap_conn_param_update_req *req;
3307 struct l2cap_conn_param_update_rsp rsp;
3308 u16 min, max, latency, to_multiplier, cmd_len;
2ce603eb 3309 int err;
de73115a
CT		 3310
3311 if (!(hcon->link_mode & HCI_LM_MASTER))
3312 return -EINVAL;
3313
3314 cmd_len = __le16_to_cpu(cmd->len);
3315 if (cmd_len != sizeof(struct l2cap_conn_param_update_req))
3316 return -EPROTO;
3317
3318 req = (struct l2cap_conn_param_update_req *) data;
e2174ca4
GP		 3319 min = __le16_to_cpu(req->min);
3320 max = __le16_to_cpu(req->max);
de73115a
CT		 3321 latency = __le16_to_cpu(req->latency);
3322 to_multiplier = __le16_to_cpu(req->to_multiplier);
3323
3324 BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x",
3325 min, max, latency, to_multiplier);
3326
3327 memset(&rsp, 0, sizeof(rsp));
2ce603eb
CT		 3328
3329 err = l2cap_check_conn_param(min, max, latency, to_multiplier);
3330 if (err)
de73115a
CT		 3331 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_REJECTED);
3332 else
3333 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED);
3334
3335 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP,
3336 sizeof(rsp), &rsp);
3337
2ce603eb
CT		 3338 if (!err)
3339 hci_le_conn_update(hcon, min, max, latency, to_multiplier);
3340
de73115a
CT		 3341 return 0;
3342}
3343
3300d9a9
CT		 3344static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
3345 struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
3346{
3347 int err = 0;
3348
3349 switch (cmd->code) {
3350 case L2CAP_COMMAND_REJ:
3351 l2cap_command_rej(conn, cmd, data);
3352 break;
3353
3354 case L2CAP_CONN_REQ:
3355 err = l2cap_connect_req(conn, cmd, data);
3356 break;
3357
3358 case L2CAP_CONN_RSP:
3359 err = l2cap_connect_rsp(conn, cmd, data);
3360 break;
3361
3362 case L2CAP_CONF_REQ:
3363 err = l2cap_config_req(conn, cmd, cmd_len, data);
3364 break;
3365
3366 case L2CAP_CONF_RSP:
3367 err = l2cap_config_rsp(conn, cmd, data);
3368 break;
3369
3370 case L2CAP_DISCONN_REQ:
3371 err = l2cap_disconnect_req(conn, cmd, data);
3372 break;
3373
3374 case L2CAP_DISCONN_RSP:
3375 err = l2cap_disconnect_rsp(conn, cmd, data);
3376 break;
3377
3378 case L2CAP_ECHO_REQ:
3379 l2cap_send_cmd(conn, cmd->ident, L2CAP_ECHO_RSP, cmd_len, data);
3380 break;
3381
3382 case L2CAP_ECHO_RSP:
3383 break;
3384
3385 case L2CAP_INFO_REQ:
3386 err = l2cap_information_req(conn, cmd, data);
3387 break;
3388
3389 case L2CAP_INFO_RSP:
3390 err = l2cap_information_rsp(conn, cmd, data);
3391 break;
3392
f94ff6ff
MM		 3393 case L2CAP_CREATE_CHAN_REQ:
3394 err = l2cap_create_channel_req(conn, cmd, cmd_len, data);
3395 break;
3396
3397 case L2CAP_CREATE_CHAN_RSP:
3398 err = l2cap_create_channel_rsp(conn, cmd, data);
3399 break;
3400
8d5a04a1
MM		 3401 case L2CAP_MOVE_CHAN_REQ:
3402 err = l2cap_move_channel_req(conn, cmd, cmd_len, data);
3403 break;
3404
3405 case L2CAP_MOVE_CHAN_RSP:
3406 err = l2cap_move_channel_rsp(conn, cmd, cmd_len, data);
3407 break;
3408
3409 case L2CAP_MOVE_CHAN_CFM:
3410 err = l2cap_move_channel_confirm(conn, cmd, cmd_len, data);
3411 break;
3412
3413 case L2CAP_MOVE_CHAN_CFM_RSP:
3414 err = l2cap_move_channel_confirm_rsp(conn, cmd, cmd_len, data);
3415 break;
3416
3300d9a9
CT		 3417 default:
3418 BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code);
3419 err = -EINVAL;
3420 break;
3421 }
3422
3423 return err;
3424}
3425
3426static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn,
3427 struct l2cap_cmd_hdr *cmd, u8 *data)
3428{
3429 switch (cmd->code) {
3430 case L2CAP_COMMAND_REJ:
3431 return 0;
3432
3433 case L2CAP_CONN_PARAM_UPDATE_REQ:
de73115a 3434 return l2cap_conn_param_update_req(conn, cmd, data);
3300d9a9
CT		 3435
3436 case L2CAP_CONN_PARAM_UPDATE_RSP:
3437 return 0;
3438
3439 default:
3440 BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code);
3441 return -EINVAL;
3442 }
3443}
3444
3445static inline void l2cap_sig_channel(struct l2cap_conn *conn,
3446 struct sk_buff *skb)
1da177e4
LT		 3447{
3448 u8 *data = skb->data;
3449 int len = skb->len;
3450 struct l2cap_cmd_hdr cmd;
3300d9a9 3451 int err;
1da177e4
LT		 3452
3453 l2cap_raw_recv(conn, skb);
3454
3455 while (len >= L2CAP_CMD_HDR_SIZE) {
88219a0f 3456 u16 cmd_len;
1da177e4
LT		 3457 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
3458 data += L2CAP_CMD_HDR_SIZE;
3459 len -= L2CAP_CMD_HDR_SIZE;
3460
88219a0f 3461 cmd_len = le16_to_cpu(cmd.len);
1da177e4 3462
88219a0f 3463 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len, cmd.ident);
1da177e4 3464
88219a0f 3465 if (cmd_len > len || !cmd.ident) {
1da177e4
LT		 3466 BT_DBG("corrupted command");
3467 break;
3468 }
3469
3300d9a9
CT		 3470 if (conn->hcon->type == LE_LINK)
3471 err = l2cap_le_sig_cmd(conn, &cmd, data);
3472 else
3473 err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data);
1da177e4
LT		 3474
3475 if (err) {
e2fd318e 3476 struct l2cap_cmd_rej_unk rej;
2c6d1a2e
GP		 3477
3478 BT_ERR("Wrong link type (%d)", err);
1da177e4
LT		 3479
3480 /* FIXME: Map err to a valid reason */
e2fd318e 3481 rej.reason = cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
1da177e4
LT		 3482 l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
3483 }
3484
88219a0f
AV		 3485 data += cmd_len;
3486 len -= cmd_len;
1da177e4
LT		 3487 }
3488
3489 kfree_skb(skb);
3490}
3491
47d1ec61 3492static int l2cap_check_fcs(struct l2cap_chan *chan, struct sk_buff *skb)
fcc203c3
GP		 3493{
3494 u16 our_fcs, rcv_fcs;
e4ca6d98
AE		 3495 int hdr_size;
3496
3497 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
3498 hdr_size = L2CAP_EXT_HDR_SIZE;
3499 else
3500 hdr_size = L2CAP_ENH_HDR_SIZE;
fcc203c3 3501
47d1ec61 3502 if (chan->fcs == L2CAP_FCS_CRC16) {
03a51213 3503 skb_trim(skb, skb->len - L2CAP_FCS_SIZE);
fcc203c3
GP		 3504 rcv_fcs = get_unaligned_le16(skb->data + skb->len);
3505 our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
3506
3507 if (our_fcs != rcv_fcs)
7a560e5c 3508 return -EBADMSG;
fcc203c3
GP		 3509 }
3510 return 0;
3511}
3512
525cd185 3513static inline void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan)
d5392c8f 3514{
88843ab0 3515 u32 control = 0;
d5392c8f 3516
6a026610 3517 chan->frames_sent = 0;
d5392c8f 3518
0b209fae 3519 control |= __set_reqseq(chan, chan->buffer_seq);
d5392c8f 3520
e2ab4353 3521 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
ab784b73 3522 control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR);
525cd185 3523 l2cap_send_sframe(chan, control);
e2ab4353 3524 set_bit(CONN_RNR_SENT, &chan->conn_state);
d5392c8f
GP		 3525 }
3526
e2ab4353 3527 if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
525cd185 3528 l2cap_retransmit_frames(chan);
d5392c8f 3529
525cd185 3530 l2cap_ertm_send(chan);
d5392c8f 3531
e2ab4353 3532 if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state) &&
6a026610 3533 chan->frames_sent == 0) {
ab784b73 3534 control |= __set_ctrl_super(chan, L2CAP_SUPER_RR);
525cd185 3535 l2cap_send_sframe(chan, control);
d5392c8f
GP		 3536 }
3537}
3538
fb45de7d 3539static int l2cap_add_to_srej_queue(struct l2cap_chan *chan, struct sk_buff *skb, u16 tx_seq, u8 sar)
8f17154f
GP		 3540{
3541 struct sk_buff *next_skb;
bfbacc11 3542 int tx_seq_offset, next_tx_seq_offset;
8f17154f
GP		 3543
3544 bt_cb(skb)->tx_seq = tx_seq;
3545 bt_cb(skb)->sar = sar;
3546
f1c6775b 3547 next_skb = skb_peek(&chan->srej_q);
8f17154f 3548
836be934 3549 tx_seq_offset = __seq_offset(chan, tx_seq, chan->buffer_seq);
bfbacc11 3550
039d9572 3551 while (next_skb) {
9b53350d
JPRV		 3552 if (bt_cb(next_skb)->tx_seq == tx_seq)
3553 return -EINVAL;
3554
836be934
AE		 3555 next_tx_seq_offset = __seq_offset(chan,
3556 bt_cb(next_skb)->tx_seq, chan->buffer_seq);
bfbacc11
JPRV		 3557
3558 if (next_tx_seq_offset > tx_seq_offset) {
f1c6775b 3559 __skb_queue_before(&chan->srej_q, next_skb, skb);
9b53350d 3560 return 0;
8f17154f
GP		 3561 }
3562
f1c6775b 3563 if (skb_queue_is_last(&chan->srej_q, next_skb))
039d9572
SJ		 3564 next_skb = NULL;
3565 else
3566 next_skb = skb_queue_next(&chan->srej_q, next_skb);
3567 }
8f17154f 3568
f1c6775b 3569 __skb_queue_tail(&chan->srej_q, skb);
9b53350d
JPRV		 3570
3571 return 0;
8f17154f
GP		 3572}
3573
84084a31
MM		 3574static void append_skb_frag(struct sk_buff *skb,
3575 struct sk_buff *new_frag, struct sk_buff **last_frag)
18778a63 3576{
84084a31
MM		 3577 /* skb->len reflects data in skb as well as all fragments
3578 * skb->data_len reflects only data in fragments
3579 */
3580 if (!skb_has_frag_list(skb))
3581 skb_shinfo(skb)->frag_list = new_frag;
3582
3583 new_frag->next = NULL;
3584
3585 (*last_frag)->next = new_frag;
3586 *last_frag = new_frag;
3587
3588 skb->len += new_frag->len;
3589 skb->data_len += new_frag->len;
3590 skb->truesize += new_frag->truesize;
3591}
3592
88843ab0 3593static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u32 control)
84084a31
MM		 3594{
3595 int err = -EINVAL;
18778a63 3596
7e0ef6ee
AE		 3597 switch (__get_ctrl_sar(chan, control)) {
3598 case L2CAP_SAR_UNSEGMENTED:
84084a31
MM		 3599 if (chan->sdu)
3600 break;
18778a63 3601
84084a31
MM		 3602 err = chan->ops->recv(chan->data, skb);
3603 break;
18778a63 3604
7e0ef6ee 3605 case L2CAP_SAR_START:
84084a31
MM		 3606 if (chan->sdu)
3607 break;
18778a63 3608
6f61fd47 3609 chan->sdu_len = get_unaligned_le16(skb->data);
03a51213 3610 skb_pull(skb, L2CAP_SDULEN_SIZE);
18778a63 3611
84084a31
MM		 3612 if (chan->sdu_len > chan->imtu) {
3613 err = -EMSGSIZE;
3614 break;
3615 }
1890d36b 3616
84084a31
MM		 3617 if (skb->len >= chan->sdu_len)
3618 break;
18778a63 3619
84084a31
MM		 3620 chan->sdu = skb;
3621 chan->sdu_last_frag = skb;
18778a63 3622
84084a31
MM		 3623 skb = NULL;
3624 err = 0;
18778a63
GP		 3625 break;
3626
7e0ef6ee 3627 case L2CAP_SAR_CONTINUE:
6f61fd47 3628 if (!chan->sdu)
84084a31 3629 break;
18778a63 3630
84084a31
MM		 3631 append_skb_frag(chan->sdu, skb,
3632 &chan->sdu_last_frag);
3633 skb = NULL;
18778a63 3634
84084a31
MM		 3635 if (chan->sdu->len >= chan->sdu_len)
3636 break;
4178ba46 3637
84084a31 3638 err = 0;
18778a63
GP		 3639 break;
3640
7e0ef6ee 3641 case L2CAP_SAR_END:
6f61fd47 3642 if (!chan->sdu)
84084a31 3643 break;
18778a63 3644
84084a31
MM		 3645 append_skb_frag(chan->sdu, skb,
3646 &chan->sdu_last_frag);
3647 skb = NULL;
4178ba46 3648
84084a31
MM		 3649 if (chan->sdu->len != chan->sdu_len)
3650 break;
18778a63 3651
84084a31 3652 err = chan->ops->recv(chan->data, chan->sdu);
1890d36b 3653
84084a31
MM		 3654 if (!err) {
3655 /* Reassembly complete */
3656 chan->sdu = NULL;
3657 chan->sdu_last_frag = NULL;
3658 chan->sdu_len = 0;
1890d36b 3659 }
18778a63
GP		 3660 break;
3661 }
3662
84084a31
MM		 3663 if (err) {
3664 kfree_skb(skb);
3665 kfree_skb(chan->sdu);
3666 chan->sdu = NULL;
3667 chan->sdu_last_frag = NULL;
3668 chan->sdu_len = 0;
3669 }
18778a63 3670
84084a31 3671 return err;
18778a63
GP		 3672}
3673
26f880d2 3674static void l2cap_ertm_enter_local_busy(struct l2cap_chan *chan)
712132eb 3675{
88843ab0 3676 u32 control;
712132eb 3677
26f880d2 3678 BT_DBG("chan %p, Enter local busy", chan);
712132eb 3679
26f880d2
MM		 3680 set_bit(CONN_LOCAL_BUSY, &chan->conn_state);
3681
0b209fae 3682 control = __set_reqseq(chan, chan->buffer_seq);
ab784b73 3683 control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR);
26f880d2
MM		 3684 l2cap_send_sframe(chan, control);
3685
3686 set_bit(CONN_RNR_SENT, &chan->conn_state);
3687
3688 __clear_ack_timer(chan);
3689}
3690
3691static void l2cap_ertm_exit_local_busy(struct l2cap_chan *chan)
3692{
88843ab0 3693 u32 control;
712132eb 3694
e2ab4353 3695 if (!test_bit(CONN_RNR_SENT, &chan->conn_state))
712132eb
GP		 3696 goto done;
3697
0b209fae 3698 control = __set_reqseq(chan, chan->buffer_seq);
e3781735 3699 control |= __set_ctrl_poll(chan);
ab784b73 3700 control |= __set_ctrl_super(chan, L2CAP_SUPER_RR);
525cd185 3701 l2cap_send_sframe(chan, control);
6a026610 3702 chan->retry_count = 1;
712132eb 3703
1a09bcb9
GP		 3704 __clear_retrans_timer(chan);
3705 __set_monitor_timer(chan);
712132eb 3706
e2ab4353 3707 set_bit(CONN_WAIT_F, &chan->conn_state);
712132eb
GP		 3708
3709done:
e2ab4353
GP		 3710 clear_bit(CONN_LOCAL_BUSY, &chan->conn_state);
3711 clear_bit(CONN_RNR_SENT, &chan->conn_state);
712132eb 3712
49208c9c 3713 BT_DBG("chan %p, Exit local busy", chan);
712132eb
GP		 3714}
3715
e328140f 3716void l2cap_chan_busy(struct l2cap_chan *chan, int busy)
1890d36b 3717{
e328140f
MM		 3718 if (chan->mode == L2CAP_MODE_ERTM) {
3719 if (busy)
3720 l2cap_ertm_enter_local_busy(chan);
3721 else
3722 l2cap_ertm_exit_local_busy(chan);
1890d36b 3723 }
1890d36b
GP		 3724}
3725
fb45de7d 3726static void l2cap_check_srej_gap(struct l2cap_chan *chan, u16 tx_seq)
8f17154f
GP		 3727{
3728 struct sk_buff *skb;
88843ab0 3729 u32 control;
8f17154f 3730
e328140f
MM		 3731 while ((skb = skb_peek(&chan->srej_q)) &&
3732 !test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
3733 int err;
3734
8f17154f
GP		 3735 if (bt_cb(skb)->tx_seq != tx_seq)
3736 break;
3737
f1c6775b 3738 skb = skb_dequeue(&chan->srej_q);
7e0ef6ee 3739 control = __set_ctrl_sar(chan, bt_cb(skb)->sar);
84084a31 3740 err = l2cap_reassemble_sdu(chan, skb, control);
e328140f
MM		 3741
3742 if (err < 0) {
3743 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3744 break;
3745 }
3746
836be934
AE		 3747 chan->buffer_seq_srej = __next_seq(chan, chan->buffer_seq_srej);
3748 tx_seq = __next_seq(chan, tx_seq);
8f17154f
GP		 3749 }
3750}
3751
fb45de7d 3752static void l2cap_resend_srejframe(struct l2cap_chan *chan, u16 tx_seq)
8f17154f 3753{
8f17154f 3754 struct srej_list *l, *tmp;
88843ab0 3755 u32 control;
8f17154f 3756
39d5a3ee 3757 list_for_each_entry_safe(l, tmp, &chan->srej_l, list) {
8f17154f
GP		 3758 if (l->tx_seq == tx_seq) {
3759 list_del(&l->list);
3760 kfree(l);
3761 return;
3762 }
ab784b73 3763 control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ);
0b209fae 3764 control |= __set_reqseq(chan, l->tx_seq);
525cd185 3765 l2cap_send_sframe(chan, control);
8f17154f 3766 list_del(&l->list);
39d5a3ee 3767 list_add_tail(&l->list, &chan->srej_l);
8f17154f
GP		 3768 }
3769}
3770
aef89f21 3771static int l2cap_send_srejframe(struct l2cap_chan *chan, u16 tx_seq)
8f17154f 3772{
8f17154f 3773 struct srej_list *new;
88843ab0 3774 u32 control;
8f17154f 3775
42e5c802 3776 while (tx_seq != chan->expected_tx_seq) {
ab784b73 3777 control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ);
0b209fae 3778 control |= __set_reqseq(chan, chan->expected_tx_seq);
525cd185 3779 l2cap_send_sframe(chan, control);
8f17154f
GP		 3780
3781 new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC);
aef89f21
SJ		 3782 if (!new)
3783 return -ENOMEM;
3784
42e5c802 3785 new->tx_seq = chan->expected_tx_seq;
836be934
AE		 3786
3787 chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq);
3788
39d5a3ee 3789 list_add_tail(&new->list, &chan->srej_l);
8f17154f 3790 }
836be934
AE		 3791
3792 chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq);
aef89f21
SJ		 3793
3794 return 0;
8f17154f
GP		 3795}
3796
88843ab0 3797static inline int l2cap_data_channel_iframe(struct l2cap_chan *chan, u32 rx_control, struct sk_buff *skb)
1c2acffb 3798{
fb45de7d 3799 u16 tx_seq = __get_txseq(chan, rx_control);
0b209fae 3800 u16 req_seq = __get_reqseq(chan, rx_control);
7e0ef6ee 3801 u8 sar = __get_ctrl_sar(chan, rx_control);
f6337c77 3802 int tx_seq_offset, expected_tx_seq_offset;
47d1ec61 3803 int num_to_ack = (chan->tx_win/6) + 1;
1c2acffb
GP		 3804 int err = 0;
3805
88843ab0 3806 BT_DBG("chan %p len %d tx_seq %d rx_control 0x%8.8x", chan, skb->len,
525cd185 3807 tx_seq, rx_control);
1c2acffb 3808
03f6715d 3809 if (__is_ctrl_final(chan, rx_control) &&
e2ab4353 3810 test_bit(CONN_WAIT_F, &chan->conn_state)) {
1a09bcb9 3811 __clear_monitor_timer(chan);
6a026610 3812 if (chan->unacked_frames > 0)
1a09bcb9 3813 __set_retrans_timer(chan);
e2ab4353 3814 clear_bit(CONN_WAIT_F, &chan->conn_state);
1d8f5d16
GP		 3815 }
3816
42e5c802
GP		 3817 chan->expected_ack_seq = req_seq;
3818 l2cap_drop_acked_frames(chan);
9f121a5a 3819
836be934 3820 tx_seq_offset = __seq_offset(chan, tx_seq, chan->buffer_seq);
9b53350d
JPRV		 3821
3822 /* invalid tx_seq */
47d1ec61 3823 if (tx_seq_offset >= chan->tx_win) {
8c1d787b 3824 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
9b53350d
JPRV		 3825 goto drop;
3826 }
3827
e2ab4353 3828 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state))
1890d36b
GP		 3829 goto drop;
3830
02f1b641
MM		 3831 if (tx_seq == chan->expected_tx_seq)
3832 goto expected;
3833
e2ab4353 3834 if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
8f17154f 3835 struct srej_list *first;
30afb5b2 3836
39d5a3ee 3837 first = list_first_entry(&chan->srej_l,
8f17154f
GP		 3838 struct srej_list, list);
3839 if (tx_seq == first->tx_seq) {
42e5c802 3840 l2cap_add_to_srej_queue(chan, skb, tx_seq, sar);
525cd185 3841 l2cap_check_srej_gap(chan, tx_seq);
8f17154f
GP		 3842
3843 list_del(&first->list);
3844 kfree(first);
3845
39d5a3ee 3846 if (list_empty(&chan->srej_l)) {
42e5c802 3847 chan->buffer_seq = chan->buffer_seq_srej;
e2ab4353 3848 clear_bit(CONN_SREJ_SENT, &chan->conn_state);
525cd185 3849 l2cap_send_ack(chan);
49208c9c 3850 BT_DBG("chan %p, Exit SREJ_SENT", chan);
8f17154f
GP		 3851 }
3852 } else {
3853 struct srej_list *l;
9b53350d
JPRV		 3854
3855 /* duplicated tx_seq */
42e5c802 3856 if (l2cap_add_to_srej_queue(chan, skb, tx_seq, sar) < 0)
9b53350d 3857 goto drop;
8f17154f 3858
39d5a3ee 3859 list_for_each_entry(l, &chan->srej_l, list) {
8f17154f 3860 if (l->tx_seq == tx_seq) {
525cd185 3861 l2cap_resend_srejframe(chan, tx_seq);
8f17154f
GP		 3862 return 0;
3863 }
3864 }
aef89f21
SJ		 3865
3866 err = l2cap_send_srejframe(chan, tx_seq);
3867 if (err < 0) {
3868 l2cap_send_disconn_req(chan->conn, chan, -err);
3869 return err;
3870 }
30afb5b2
GP		 3871 }
3872 } else {
836be934
AE		 3873 expected_tx_seq_offset = __seq_offset(chan,
3874 chan->expected_tx_seq, chan->buffer_seq);
9b53350d
JPRV		 3875
3876 /* duplicated tx_seq */
3877 if (tx_seq_offset < expected_tx_seq_offset)
3878 goto drop;
3879
e2ab4353 3880 set_bit(CONN_SREJ_SENT, &chan->conn_state);
1c2acffb 3881
49208c9c 3882 BT_DBG("chan %p, Enter SREJ", chan);
0e98958d 3883
39d5a3ee 3884 INIT_LIST_HEAD(&chan->srej_l);
42e5c802 3885 chan->buffer_seq_srej = chan->buffer_seq;
8f17154f 3886
f1c6775b 3887 __skb_queue_head_init(&chan->srej_q);
42e5c802 3888 l2cap_add_to_srej_queue(chan, skb, tx_seq, sar);
8f17154f 3889
e2ab4353 3890 set_bit(CONN_SEND_PBIT, &chan->conn_state);
ef54fd93 3891
aef89f21
SJ		 3892 err = l2cap_send_srejframe(chan, tx_seq);
3893 if (err < 0) {
3894 l2cap_send_disconn_req(chan->conn, chan, -err);
3895 return err;
3896 }
7fe9b298 3897
1a09bcb9 3898 __clear_ack_timer(chan);
1c2acffb 3899 }
30afb5b2
GP		 3900 return 0;
3901
8f17154f 3902expected:
836be934 3903 chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq);
8f17154f 3904
e2ab4353 3905 if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
3b1a9f3f
GP		 3906 bt_cb(skb)->tx_seq = tx_seq;
3907 bt_cb(skb)->sar = sar;
f1c6775b 3908 __skb_queue_tail(&chan->srej_q, skb);
8f17154f
GP		 3909 return 0;
3910 }
3911
84084a31 3912 err = l2cap_reassemble_sdu(chan, skb, rx_control);
836be934
AE		 3913 chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
3914
e328140f
MM		 3915 if (err < 0) {
3916 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3917 return err;
3918 }
2ece3684 3919
03f6715d 3920 if (__is_ctrl_final(chan, rx_control)) {
e2ab4353 3921 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
525cd185 3922 l2cap_retransmit_frames(chan);
4ec10d97
GP		 3923 }
3924
c1b4f43b 3925
6a026610
GP		 3926 chan->num_acked = (chan->num_acked + 1) % num_to_ack;
3927 if (chan->num_acked == num_to_ack - 1)
525cd185 3928 l2cap_send_ack(chan);
4d611e4d
GP		 3929 else
3930 __set_ack_timer(chan);
9e917af1 3931
8f17154f 3932 return 0;
9b53350d
JPRV		 3933
3934drop:
3935 kfree_skb(skb);
3936 return 0;
1c2acffb
GP		 3937}
3938
88843ab0 3939static inline void l2cap_data_channel_rrframe(struct l2cap_chan *chan, u32 rx_control)
1c2acffb 3940{
88843ab0 3941 BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan,
0b209fae 3942 __get_reqseq(chan, rx_control), rx_control);
0e98958d 3943
0b209fae 3944 chan->expected_ack_seq = __get_reqseq(chan, rx_control);
42e5c802 3945 l2cap_drop_acked_frames(chan);
1c2acffb 3946
e3781735 3947 if (__is_ctrl_poll(chan, rx_control)) {
e2ab4353
GP		 3948 set_bit(CONN_SEND_FBIT, &chan->conn_state);
3949 if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
3950 if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
6a026610 3951 (chan->unacked_frames > 0))
1a09bcb9 3952 __set_retrans_timer(chan);
05fbd89d 3953
e2ab4353 3954 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
525cd185 3955 l2cap_send_srejtail(chan);
05fbd89d 3956 } else {
525cd185 3957 l2cap_send_i_or_rr_or_rnr(chan);
05fbd89d 3958 }
1d8f5d16 3959
03f6715d 3960 } else if (__is_ctrl_final(chan, rx_control)) {
e2ab4353 3961 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
4ec10d97 3962
e2ab4353 3963 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
525cd185 3964 l2cap_retransmit_frames(chan);
2246b2f1 3965
e072745f 3966 } else {
e2ab4353 3967 if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
6a026610 3968 (chan->unacked_frames > 0))
1a09bcb9 3969 __set_retrans_timer(chan);
1c2acffb 3970
e2ab4353
GP		 3971 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3972 if (test_bit(CONN_SREJ_SENT, &chan->conn_state))
525cd185 3973 l2cap_send_ack(chan);
894718a6 3974 else
525cd185 3975 l2cap_ertm_send(chan);
e072745f
GP		 3976 }
3977}
2246b2f1 3978
88843ab0 3979static inline void l2cap_data_channel_rejframe(struct l2cap_chan *chan, u32 rx_control)
e072745f 3980{
0b209fae 3981 u16 tx_seq = __get_reqseq(chan, rx_control);
30afb5b2 3982
88843ab0 3983 BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control);
0e98958d 3984
e2ab4353 3985 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
e072745f 3986
42e5c802
GP		 3987 chan->expected_ack_seq = tx_seq;
3988 l2cap_drop_acked_frames(chan);
e072745f 3989
03f6715d 3990 if (__is_ctrl_final(chan, rx_control)) {
e2ab4353 3991 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
525cd185 3992 l2cap_retransmit_frames(chan);
e072745f 3993 } else {
525cd185 3994 l2cap_retransmit_frames(chan);
30afb5b2 3995
e2ab4353
GP		 3996 if (test_bit(CONN_WAIT_F, &chan->conn_state))
3997 set_bit(CONN_REJ_ACT, &chan->conn_state);
e072745f
GP		 3998 }
3999}
88843ab0 4000static inline void l2cap_data_channel_srejframe(struct l2cap_chan *chan, u32 rx_control)
e072745f 4001{
0b209fae 4002 u16 tx_seq = __get_reqseq(chan, rx_control);
30afb5b2 4003
88843ab0 4004 BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control);
0e98958d 4005
e2ab4353 4006 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
2246b2f1 4007
e3781735 4008 if (__is_ctrl_poll(chan, rx_control)) {
42e5c802
GP		 4009 chan->expected_ack_seq = tx_seq;
4010 l2cap_drop_acked_frames(chan);
3cb123d1 4011
e2ab4353 4012 set_bit(CONN_SEND_FBIT, &chan->conn_state);
525cd185 4013 l2cap_retransmit_one_frame(chan, tx_seq);
dfc909be 4014
525cd185 4015 l2cap_ertm_send(chan);
dfc909be 4016
e2ab4353 4017 if (test_bit(CONN_WAIT_F, &chan->conn_state)) {
6a026610 4018 chan->srej_save_reqseq = tx_seq;
e2ab4353 4019 set_bit(CONN_SREJ_ACT, &chan->conn_state);
ef54fd93 4020 }
03f6715d 4021 } else if (__is_ctrl_final(chan, rx_control)) {
e2ab4353 4022 if (test_bit(CONN_SREJ_ACT, &chan->conn_state) &&
6a026610 4023 chan->srej_save_reqseq == tx_seq)
e2ab4353 4024 clear_bit(CONN_SREJ_ACT, &chan->conn_state);
e072745f 4025 else
525cd185 4026 l2cap_retransmit_one_frame(chan, tx_seq);
e072745f 4027 } else {
525cd185 4028 l2cap_retransmit_one_frame(chan, tx_seq);
e2ab4353 4029 if (test_bit(CONN_WAIT_F, &chan->conn_state)) {
6a026610 4030 chan->srej_save_reqseq = tx_seq;
e2ab4353 4031 set_bit(CONN_SREJ_ACT, &chan->conn_state);
ef54fd93 4032 }
e072745f
GP		 4033 }
4034}
4035
88843ab0 4036static inline void l2cap_data_channel_rnrframe(struct l2cap_chan *chan, u32 rx_control)
e072745f 4037{
0b209fae 4038 u16 tx_seq = __get_reqseq(chan, rx_control);
e072745f 4039
88843ab0 4040 BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control);
0e98958d 4041
e2ab4353 4042 set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
42e5c802
GP		 4043 chan->expected_ack_seq = tx_seq;
4044 l2cap_drop_acked_frames(chan);
e072745f 4045
e3781735 4046 if (__is_ctrl_poll(chan, rx_control))
e2ab4353 4047 set_bit(CONN_SEND_FBIT, &chan->conn_state);
3cb123d1 4048
e2ab4353 4049 if (!test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
1a09bcb9 4050 __clear_retrans_timer(chan);
e3781735 4051 if (__is_ctrl_poll(chan, rx_control))
525cd185 4052 l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_FINAL);
99b0d4b7 4053 return;
e072745f 4054 }
99b0d4b7 4055
e3781735 4056 if (__is_ctrl_poll(chan, rx_control)) {
525cd185 4057 l2cap_send_srejtail(chan);
ab784b73
AE		 4058 } else {
4059 rx_control = __set_ctrl_super(chan, L2CAP_SUPER_RR);
4060 l2cap_send_sframe(chan, rx_control);
4061 }
e072745f
GP		 4062}
4063
88843ab0 4064static inline int l2cap_data_channel_sframe(struct l2cap_chan *chan, u32 rx_control, struct sk_buff *skb)
e072745f 4065{
88843ab0 4066 BT_DBG("chan %p rx_control 0x%8.8x len %d", chan, rx_control, skb->len);
e072745f 4067
03f6715d 4068 if (__is_ctrl_final(chan, rx_control) &&
e2ab4353 4069 test_bit(CONN_WAIT_F, &chan->conn_state)) {
1a09bcb9 4070 __clear_monitor_timer(chan);
6a026610 4071 if (chan->unacked_frames > 0)
1a09bcb9 4072 __set_retrans_timer(chan);
e2ab4353 4073 clear_bit(CONN_WAIT_F, &chan->conn_state);
e072745f
GP		 4074 }
4075
ab784b73
AE		 4076 switch (__get_ctrl_super(chan, rx_control)) {
4077 case L2CAP_SUPER_RR:
525cd185 4078 l2cap_data_channel_rrframe(chan, rx_control);
8f17154f
GP		 4079 break;
4080
ab784b73 4081 case L2CAP_SUPER_REJ:
525cd185 4082 l2cap_data_channel_rejframe(chan, rx_control);
e072745f 4083 break;
2246b2f1 4084
ab784b73 4085 case L2CAP_SUPER_SREJ:
525cd185 4086 l2cap_data_channel_srejframe(chan, rx_control);
e072745f
GP		 4087 break;
4088
ab784b73 4089 case L2CAP_SUPER_RNR:
525cd185 4090 l2cap_data_channel_rnrframe(chan, rx_control);
1c2acffb
GP		 4091 break;
4092 }
4093
faaebd19 4094 kfree_skb(skb);
1c2acffb
GP		 4095 return 0;
4096}
4097
218bb9df
GP		 4098static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb)
4099{
525cd185 4100 struct l2cap_chan *chan = l2cap_pi(sk)->chan;
88843ab0 4101 u32 control;
0b209fae 4102 u16 req_seq;
218bb9df
GP		 4103 int len, next_tx_seq_offset, req_seq_offset;
4104
88843ab0
AE		 4105 control = __get_control(chan, skb->data);
4106 skb_pull(skb, __ctrl_size(chan));
218bb9df
GP		 4107 len = skb->len;
4108
4109 /*
4110 * We can just drop the corrupted I-frame here.
4111 * Receiver will miss it and start proper recovery
4112 * procedures and ask retransmission.
4113 */
47d1ec61 4114 if (l2cap_check_fcs(chan, skb))
218bb9df
GP		 4115 goto drop;
4116
793c2f1c 4117 if (__is_sar_start(chan, control) && !__is_sframe(chan, control))
03a51213 4118 len -= L2CAP_SDULEN_SIZE;
218bb9df 4119
47d1ec61 4120 if (chan->fcs == L2CAP_FCS_CRC16)
03a51213 4121 len -= L2CAP_FCS_SIZE;
218bb9df 4122
47d1ec61 4123 if (len > chan->mps) {
8c1d787b 4124 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
218bb9df
GP		 4125 goto drop;
4126 }
4127
0b209fae 4128 req_seq = __get_reqseq(chan, control);
218bb9df 4129
836be934
AE		 4130 req_seq_offset = __seq_offset(chan, req_seq, chan->expected_ack_seq);
4131
4132 next_tx_seq_offset = __seq_offset(chan, chan->next_tx_seq,
4133 chan->expected_ack_seq);
218bb9df
GP		 4134
4135 /* check for invalid req-seq */
4136 if (req_seq_offset > next_tx_seq_offset) {
8c1d787b 4137 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
218bb9df
GP		 4138 goto drop;
4139 }
4140
793c2f1c 4141 if (!__is_sframe(chan, control)) {
218bb9df 4142 if (len < 0) {
8c1d787b 4143 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
218bb9df
GP		 4144 goto drop;
4145 }
4146
525cd185 4147 l2cap_data_channel_iframe(chan, control, skb);
218bb9df
GP		 4148 } else {
4149 if (len != 0) {
4150 BT_ERR("%d", len);
8c1d787b 4151 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
218bb9df
GP		 4152 goto drop;
4153 }
4154
525cd185 4155 l2cap_data_channel_sframe(chan, control, skb);
218bb9df
GP		 4156 }
4157
4158 return 0;
4159
4160drop:
4161 kfree_skb(skb);
4162 return 0;
4163}
4164
1da177e4
LT		 4165static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
4166{
48454079 4167 struct l2cap_chan *chan;
bf734843 4168 struct sock *sk = NULL;
88843ab0 4169 u32 control;
fb45de7d 4170 u16 tx_seq;
218bb9df 4171 int len;
1da177e4 4172
baa7e1fa 4173 chan = l2cap_get_chan_by_scid(conn, cid);
48454079 4174 if (!chan) {
1da177e4
LT		 4175 BT_DBG("unknown cid 0x%4.4x", cid);
4176 goto drop;
4177 }
4178
48454079 4179 sk = chan->sk;
6840ed07 4180
49208c9c 4181 BT_DBG("chan %p, len %d", chan, skb->len);
1da177e4 4182
89bc500e 4183 if (chan->state != BT_CONNECTED)
1da177e4
LT		 4184 goto drop;
4185
0c1bc5c6 4186 switch (chan->mode) {
1c2acffb
GP		 4187 case L2CAP_MODE_BASIC:
4188 /* If socket recv buffers overflows we drop data here
4189 * which is *bad* because L2CAP has to be reliable.
4190 * But we don't have any other choice. L2CAP doesn't
4191 * provide flow control mechanism. */
1da177e4 4192
0c1bc5c6 4193 if (chan->imtu < skb->len)
1c2acffb 4194 goto drop;
1da177e4 4195
23070494 4196 if (!chan->ops->recv(chan->data, skb))
1c2acffb
GP		 4197 goto done;
4198 break;
4199
4200 case L2CAP_MODE_ERTM:
eb403a1b 4201 l2cap_ertm_data_rcv(sk, skb);
1c2acffb 4202
fcafde2e 4203 goto done;
1c2acffb 4204
6840ed07 4205 case L2CAP_MODE_STREAMING:
88843ab0
AE		 4206 control = __get_control(chan, skb->data);
4207 skb_pull(skb, __ctrl_size(chan));
6840ed07
GP		 4208 len = skb->len;
4209
47d1ec61 4210 if (l2cap_check_fcs(chan, skb))
26000089
GP		 4211 goto drop;
4212
7e0ef6ee 4213 if (__is_sar_start(chan, control))
03a51213 4214 len -= L2CAP_SDULEN_SIZE;
6840ed07 4215
47d1ec61 4216 if (chan->fcs == L2CAP_FCS_CRC16)
03a51213 4217 len -= L2CAP_FCS_SIZE;
fcc203c3 4218
793c2f1c 4219 if (len > chan->mps || len < 0 || __is_sframe(chan, control))
6840ed07
GP		 4220 goto drop;
4221
fb45de7d 4222 tx_seq = __get_txseq(chan, control);
6840ed07 4223
84084a31
MM		 4224 if (chan->expected_tx_seq != tx_seq) {
4225 /* Frame(s) missing - must discard partial SDU */
4226 kfree_skb(chan->sdu);
4227 chan->sdu = NULL;
4228 chan->sdu_last_frag = NULL;
4229 chan->sdu_len = 0;
6840ed07 4230
84084a31
MM		 4231 /* TODO: Notify userland of missing data */
4232 }
4233
836be934 4234 chan->expected_tx_seq = __next_seq(chan, tx_seq);
84084a31
MM		 4235
4236 if (l2cap_reassemble_sdu(chan, skb, control) == -EMSGSIZE)
4237 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
6840ed07
GP		 4238
4239 goto done;
4240
1c2acffb 4241 default:
0c1bc5c6 4242 BT_DBG("chan %p: bad mode 0x%2.2x", chan, chan->mode);
1c2acffb
GP		 4243 break;
4244 }
1da177e4
LT		 4245
4246drop:
4247 kfree_skb(skb);
4248
4249done:
0139418c 4250 if (sk)
aa2ac881 4251 release_sock(sk);
0139418c 4252
1da177e4
LT		 4253 return 0;
4254}
4255
8e036fc3 4256static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb)
1da177e4 4257{
6dcae1ea 4258 struct sock *sk = NULL;
23691d75 4259 struct l2cap_chan *chan;
1da177e4 4260
23691d75
GP		 4261 chan = l2cap_global_chan_by_psm(0, psm, conn->src);
4262 if (!chan)
1da177e4
LT		 4263 goto drop;
4264
23691d75
GP		 4265 sk = chan->sk;
4266
aa2ac881 4267 lock_sock(sk);
e0f0cb56 4268
1da177e4
LT		 4269 BT_DBG("sk %p, len %d", sk, skb->len);
4270
89bc500e 4271 if (chan->state != BT_BOUND && chan->state != BT_CONNECTED)
1da177e4
LT		 4272 goto drop;
4273
e13e21dc 4274 if (chan->imtu < skb->len)
1da177e4
LT		 4275 goto drop;
4276
23070494 4277 if (!chan->ops->recv(chan->data, skb))
1da177e4
LT		 4278 goto done;
4279
4280drop:
4281 kfree_skb(skb);
4282
4283done:
af05b30b 4284 if (sk)
aa2ac881 4285 release_sock(sk);
1da177e4
LT		 4286 return 0;
4287}
4288
9f69bda6
GP		 4289static inline int l2cap_att_channel(struct l2cap_conn *conn, __le16 cid, struct sk_buff *skb)
4290{
6dcae1ea 4291 struct sock *sk = NULL;
23691d75 4292 struct l2cap_chan *chan;
9f69bda6 4293
23691d75
GP		 4294 chan = l2cap_global_chan_by_scid(0, cid, conn->src);
4295 if (!chan)
9f69bda6
GP		 4296 goto drop;
4297
23691d75
GP		 4298 sk = chan->sk;
4299
aa2ac881 4300 lock_sock(sk);
9f69bda6
GP		 4301
4302 BT_DBG("sk %p, len %d", sk, skb->len);
4303
89bc500e 4304 if (chan->state != BT_BOUND && chan->state != BT_CONNECTED)
9f69bda6
GP		 4305 goto drop;
4306
e13e21dc 4307 if (chan->imtu < skb->len)
9f69bda6
GP		 4308 goto drop;
4309
23070494 4310 if (!chan->ops->recv(chan->data, skb))
9f69bda6
GP		 4311 goto done;
4312
4313drop:
4314 kfree_skb(skb);
4315
4316done:
4317 if (sk)
aa2ac881 4318 release_sock(sk);
9f69bda6
GP		 4319 return 0;
4320}
4321
1da177e4
LT		 4322static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
4323{
4324 struct l2cap_hdr *lh = (void *) skb->data;
8e036fc3
AV		 4325 u16 cid, len;
4326 __le16 psm;
1da177e4
LT		 4327
4328 skb_pull(skb, L2CAP_HDR_SIZE);
4329 cid = __le16_to_cpu(lh->cid);
4330 len = __le16_to_cpu(lh->len);
4331
1c2acffb
GP		 4332 if (len != skb->len) {
4333 kfree_skb(skb);
4334 return;
4335 }
4336
1da177e4
LT		 4337 BT_DBG("len %d, cid 0x%4.4x", len, cid);
4338
4339 switch (cid) {
3300d9a9 4340 case L2CAP_CID_LE_SIGNALING:
8db4dc46 4341 case L2CAP_CID_SIGNALING:
1da177e4
LT		 4342 l2cap_sig_channel(conn, skb);
4343 break;
4344
8db4dc46 4345 case L2CAP_CID_CONN_LESS:
1b7bf4ed 4346 psm = get_unaligned_le16(skb->data);
1da177e4
LT		 4347 skb_pull(skb, 2);
4348 l2cap_conless_channel(conn, psm, skb);
4349 break;
4350
9f69bda6
GP		 4351 case L2CAP_CID_LE_DATA:
4352 l2cap_att_channel(conn, cid, skb);
4353 break;
4354
b501d6a1
AB		 4355 case L2CAP_CID_SMP:
4356 if (smp_sig_channel(conn, skb))
4357 l2cap_conn_del(conn->hcon, EACCES);
4358 break;
4359
1da177e4
LT		 4360 default:
4361 l2cap_data_channel(conn, cid, skb);
4362 break;
4363 }
4364}
4365
4366/* ---- L2CAP interface with lower layer (HCI) ---- */
4367
4368static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
4369{
4370 int exact = 0, lm1 = 0, lm2 = 0;
23691d75 4371 struct l2cap_chan *c;
1da177e4
LT		 4372
4373 if (type != ACL_LINK)
963cf687 4374 return -EINVAL;
1da177e4
LT		 4375
4376 BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
4377
4378 /* Find listening sockets and check their link_mode */
23691d75
GP		 4379 read_lock(&chan_list_lock);
4380 list_for_each_entry(c, &chan_list, global_l) {
4381 struct sock *sk = c->sk;
4343478f 4382
89bc500e 4383 if (c->state != BT_LISTEN)
1da177e4
LT		 4384 continue;
4385
4386 if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
2af6b9d5 4387 lm1 |= HCI_LM_ACCEPT;
43bd0f32 4388 if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
2af6b9d5 4389 lm1 |= HCI_LM_MASTER;
1da177e4 4390 exact++;
2af6b9d5
MH		 4391 } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) {
4392 lm2 |= HCI_LM_ACCEPT;
43bd0f32 4393 if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
2af6b9d5
MH		 4394 lm2 |= HCI_LM_MASTER;
4395 }
1da177e4 4396 }
23691d75 4397 read_unlock(&chan_list_lock);
1da177e4
LT		 4398
4399 return exact ? lm1 : lm2;
4400}
4401
4402static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
4403{
0139418c
MH		 4404 struct l2cap_conn *conn;
4405
1da177e4
LT		 4406 BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
4407
acd7d370 4408 if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK))
963cf687 4409 return -EINVAL;
1da177e4
LT		 4410
4411 if (!status) {
1da177e4
LT		 4412 conn = l2cap_conn_add(hcon, status);
4413 if (conn)
4414 l2cap_conn_ready(conn);
0139418c 4415 } else
e175072f 4416 l2cap_conn_del(hcon, bt_to_errno(status));
1da177e4
LT		 4417
4418 return 0;
4419}
4420
2950f21a
MH		 4421static int l2cap_disconn_ind(struct hci_conn *hcon)
4422{
4423 struct l2cap_conn *conn = hcon->l2cap_data;
4424
4425 BT_DBG("hcon %p", hcon);
4426
b5694506 4427 if ((hcon->type != ACL_LINK && hcon->type != LE_LINK) || !conn)
9f5a0d7b 4428 return HCI_ERROR_REMOTE_USER_TERM;
2950f21a
MH		 4429
4430 return conn->disc_reason;
4431}
4432
4433static int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
1da177e4
LT		 4434{
4435 BT_DBG("hcon %p reason %d", hcon, reason);
4436
acd7d370 4437 if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK))
963cf687 4438 return -EINVAL;
1da177e4 4439
e175072f 4440 l2cap_conn_del(hcon, bt_to_errno(reason));
0139418c 4441
1da177e4
LT		 4442 return 0;
4443}
4444
4343478f 4445static inline void l2cap_check_encryption(struct l2cap_chan *chan, u8 encrypt)
f62e4323 4446{
715ec005 4447 if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED)
255c7601
MH		 4448 return;
4449
f62e4323 4450 if (encrypt == 0x00) {
4343478f 4451 if (chan->sec_level == BT_SECURITY_MEDIUM) {
c9b66675 4452 __clear_chan_timer(chan);
f3f668b0 4453 __set_chan_timer(chan, L2CAP_ENC_TIMEOUT);
4343478f 4454 } else if (chan->sec_level == BT_SECURITY_HIGH)
0f852724 4455 l2cap_chan_close(chan, ECONNREFUSED);
f62e4323 4456 } else {
4343478f 4457 if (chan->sec_level == BT_SECURITY_MEDIUM)
c9b66675 4458 __clear_chan_timer(chan);
f62e4323
MH		 4459 }
4460}
4461
8c1b2355 4462static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
1da177e4 4463{
0139418c 4464 struct l2cap_conn *conn = hcon->l2cap_data;
48454079 4465 struct l2cap_chan *chan;
1da177e4 4466
0139418c 4467 if (!conn)
1da177e4 4468 return 0;
0139418c 4469
1da177e4
LT		 4470 BT_DBG("conn %p", conn);
4471
160dc6ac
VCG		 4472 if (hcon->type == LE_LINK) {
4473 smp_distribute_keys(conn, 0);
4474 del_timer(&conn->security_timer);
4475 }
4476
d01b2ff4 4477 mutex_lock(&conn->chan_lock);
1da177e4 4478
baa7e1fa 4479 list_for_each_entry(chan, &conn->chan_l, list) {
48454079 4480 struct sock *sk = chan->sk;
baa7e1fa 4481
1da177e4
LT		 4482 bh_lock_sock(sk);
4483
f1cb9af5
VCG		 4484 BT_DBG("chan->scid %d", chan->scid);
4485
4486 if (chan->scid == L2CAP_CID_LE_DATA) {
4487 if (!status && encrypt) {
4488 chan->sec_level = hcon->sec_level;
4489 l2cap_chan_ready(sk);
4490 }
4491
4492 bh_unlock_sock(sk);
4493 continue;
4494 }
4495
c1360a1c 4496 if (test_bit(CONF_CONNECT_PEND, &chan->conf_state)) {
6a8d3010
MH		 4497 bh_unlock_sock(sk);
4498 continue;
4499 }
4500
89bc500e
GP		 4501 if (!status && (chan->state == BT_CONNECTED ||
4502 chan->state == BT_CONFIG)) {
4343478f 4503 l2cap_check_encryption(chan, encrypt);
9719f8af
MH		 4504 bh_unlock_sock(sk);
4505 continue;
4506 }
4507
89bc500e 4508 if (chan->state == BT_CONNECT) {
b1235d79
MH		 4509 if (!status) {
4510 struct l2cap_conn_req req;
fe4128e0
GP		 4511 req.scid = cpu_to_le16(chan->scid);
4512 req.psm = chan->psm;
1da177e4 4513
fc7f8a7e 4514 chan->ident = l2cap_get_ident(conn);
c1360a1c 4515 set_bit(CONF_CONNECT_PEND, &chan->conf_state);
1da177e4 4516
fc7f8a7e 4517 l2cap_send_cmd(conn, chan->ident,
b1235d79
MH		 4518 L2CAP_CONN_REQ, sizeof(req), &req);
4519 } else {
c9b66675 4520 __clear_chan_timer(chan);
f3f668b0 4521 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
b1235d79 4522 }
89bc500e 4523 } else if (chan->state == BT_CONNECT2) {
b1235d79 4524 struct l2cap_conn_rsp rsp;
df3c3931 4525 __u16 res, stat;
1da177e4 4526
b1235d79 4527 if (!status) {
df3c3931
JH		 4528 if (bt_sk(sk)->defer_setup) {
4529 struct sock *parent = bt_sk(sk)->parent;
4530 res = L2CAP_CR_PEND;
4531 stat = L2CAP_CS_AUTHOR_PEND;
05e9a2f6
IK		 4532 if (parent)
4533 parent->sk_data_ready(parent, 0);
df3c3931 4534 } else {
05558911 4535 l2cap_state_change(chan, BT_CONFIG);
df3c3931
JH		 4536 res = L2CAP_CR_SUCCESS;
4537 stat = L2CAP_CS_NO_INFO;
4538 }
b1235d79 4539 } else {
89bc500e 4540 l2cap_state_change(chan, BT_DISCONN);
f3f668b0 4541 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
df3c3931
JH		 4542 res = L2CAP_CR_SEC_BLOCK;
4543 stat = L2CAP_CS_NO_INFO;
b1235d79
MH		 4544 }
4545
fe4128e0
GP		 4546 rsp.scid = cpu_to_le16(chan->dcid);
4547 rsp.dcid = cpu_to_le16(chan->scid);
df3c3931
JH		 4548 rsp.result = cpu_to_le16(res);
4549 rsp.status = cpu_to_le16(stat);
fc7f8a7e
GP		 4550 l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
4551 sizeof(rsp), &rsp);
b1235d79 4552 }
1da177e4
LT		 4553
4554 bh_unlock_sock(sk);
4555 }
4556
d01b2ff4 4557 mutex_unlock(&conn->chan_lock);
b1235d79 4558
1da177e4
LT		 4559 return 0;
4560}
4561
4562static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
4563{
4564 struct l2cap_conn *conn = hcon->l2cap_data;
4565
5a08ecce
AE		 4566 if (!conn)
4567 conn = l2cap_conn_add(hcon, 0);
4568
4569 if (!conn)
1da177e4
LT		 4570 goto drop;
4571
4572 BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
4573
e702112f 4574 if (!(flags & ACL_CONT)) {
1da177e4 4575 struct l2cap_hdr *hdr;
48454079 4576 struct l2cap_chan *chan;
89794813 4577 u16 cid;
1da177e4
LT		 4578 int len;
4579
4580 if (conn->rx_len) {
4581 BT_ERR("Unexpected start frame (len %d)", skb->len);
4582 kfree_skb(conn->rx_skb);
4583 conn->rx_skb = NULL;
4584 conn->rx_len = 0;
4585 l2cap_conn_unreliable(conn, ECOMM);
4586 }
4587
aae7fe22
AE		 4588 /* Start fragment always begin with Basic L2CAP header */
4589 if (skb->len < L2CAP_HDR_SIZE) {
1da177e4
LT		 4590 BT_ERR("Frame is too short (len %d)", skb->len);
4591 l2cap_conn_unreliable(conn, ECOMM);
4592 goto drop;
4593 }
4594
4595 hdr = (struct l2cap_hdr *) skb->data;
4596 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
89794813 4597 cid = __le16_to_cpu(hdr->cid);
1da177e4
LT		 4598
4599 if (len == skb->len) {
4600 /* Complete frame received */
4601 l2cap_recv_frame(conn, skb);
4602 return 0;
4603 }
4604
4605 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
4606
4607 if (skb->len > len) {
4608 BT_ERR("Frame is too long (len %d, expected len %d)",
4609 skb->len, len);
4610 l2cap_conn_unreliable(conn, ECOMM);
4611 goto drop;
4612 }
4613
baa7e1fa 4614 chan = l2cap_get_chan_by_scid(conn, cid);
89794813 4615
48454079
GP		 4616 if (chan && chan->sk) {
4617 struct sock *sk = chan->sk;
89794813 4618
0c1bc5c6 4619 if (chan->imtu < len - L2CAP_HDR_SIZE) {
48454079
GP		 4620 BT_ERR("Frame exceeding recv MTU (len %d, "
4621 "MTU %d)", len,
0c1bc5c6 4622 chan->imtu);
aa2ac881 4623 release_sock(sk);
48454079
GP		 4624 l2cap_conn_unreliable(conn, ECOMM);
4625 goto drop;
4626 }
aa2ac881 4627 release_sock(sk);
48454079 4628 }
89794813 4629
1da177e4 4630 /* Allocate skb for the complete frame (with header) */
af05b30b
GP		 4631 conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC);
4632 if (!conn->rx_skb)
1da177e4
LT		 4633 goto drop;
4634
d626f62b 4635 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
e1027a7c 4636 skb->len);
1da177e4
LT		 4637 conn->rx_len = len - skb->len;
4638 } else {
4639 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
4640
4641 if (!conn->rx_len) {
4642 BT_ERR("Unexpected continuation frame (len %d)", skb->len);
4643 l2cap_conn_unreliable(conn, ECOMM);
4644 goto drop;
4645 }
4646
4647 if (skb->len > conn->rx_len) {
4648 BT_ERR("Fragment is too long (len %d, expected %d)",
4649 skb->len, conn->rx_len);
4650 kfree_skb(conn->rx_skb);
4651 conn->rx_skb = NULL;
4652 conn->rx_len = 0;
4653 l2cap_conn_unreliable(conn, ECOMM);
4654 goto drop;
4655 }
4656
d626f62b 4657 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
e1027a7c 4658 skb->len);
1da177e4
LT		 4659 conn->rx_len -= skb->len;
4660
4661 if (!conn->rx_len) {
4662 /* Complete frame received */
4663 l2cap_recv_frame(conn, conn->rx_skb);
4664 conn->rx_skb = NULL;
4665 }
4666 }
4667
4668drop:
4669 kfree_skb(skb);
4670 return 0;
4671}
4672
aef7d97c 4673static int l2cap_debugfs_show(struct seq_file *f, void *p)
1da177e4 4674{
23691d75 4675 struct l2cap_chan *c;
1da177e4 4676
23691d75 4677 read_lock_bh(&chan_list_lock);
1da177e4 4678
23691d75
GP		 4679 list_for_each_entry(c, &chan_list, global_l) {
4680 struct sock *sk = c->sk;
101545f6 4681
903d343e 4682 seq_printf(f, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n",
aef7d97c
MH		 4683 batostr(&bt_sk(sk)->src),
4684 batostr(&bt_sk(sk)->dst),
89bc500e 4685 c->state, __le16_to_cpu(c->psm),
23691d75
GP		 4686 c->scid, c->dcid, c->imtu, c->omtu,
4687 c->sec_level, c->mode);
05558911 4688}
1da177e4 4689
23691d75 4690 read_unlock_bh(&chan_list_lock);
1da177e4 4691
aef7d97c 4692 return 0;
1da177e4
LT		 4693}
4694
aef7d97c
MH		 4695static int l2cap_debugfs_open(struct inode *inode, struct file *file)
4696{
4697 return single_open(file, l2cap_debugfs_show, inode->i_private);
4698}
4699
4700static const struct file_operations l2cap_debugfs_fops = {
4701 .open = l2cap_debugfs_open,
4702 .read = seq_read,
4703 .llseek = seq_lseek,
4704 .release = single_release,
4705};
4706
4707static struct dentry *l2cap_debugfs;
1da177e4 4708
1da177e4
LT		 4709static struct hci_proto l2cap_hci_proto = {
4710 .name = "L2CAP",
4711 .id = HCI_PROTO_L2CAP,
4712 .connect_ind = l2cap_connect_ind,
4713 .connect_cfm = l2cap_connect_cfm,
4714 .disconn_ind = l2cap_disconn_ind,
2950f21a 4715 .disconn_cfm = l2cap_disconn_cfm,
8c1b2355 4716 .security_cfm = l2cap_security_cfm,
1da177e4
LT		 4717 .recv_acldata = l2cap_recv_acldata
4718};
4719
64274518 4720int __init l2cap_init(void)
1da177e4
LT		 4721{
4722 int err;
be9d1227 4723
bb58f747 4724 err = l2cap_init_sockets();
1da177e4
LT		 4725 if (err < 0)
4726 return err;
4727
1da177e4
LT		 4728 err = hci_register_proto(&l2cap_hci_proto);
4729 if (err < 0) {
4730 BT_ERR("L2CAP protocol registration failed");
4731 bt_sock_unregister(BTPROTO_L2CAP);
4732 goto error;
4733 }
4734
aef7d97c
MH		 4735 if (bt_debugfs) {
4736 l2cap_debugfs = debugfs_create_file("l2cap", 0444,
4737 bt_debugfs, NULL, &l2cap_debugfs_fops);
4738 if (!l2cap_debugfs)
4739 BT_ERR("Failed to create L2CAP debug file");
4740 }
1da177e4 4741
1da177e4
LT		 4742 return 0;
4743
4744error:
bb58f747 4745 l2cap_cleanup_sockets();
1da177e4
LT		 4746 return err;
4747}
4748
64274518 4749void l2cap_exit(void)
1da177e4 4750{
aef7d97c 4751 debugfs_remove(l2cap_debugfs);
1da177e4 4752
1da177e4
LT		 4753 if (hci_unregister_proto(&l2cap_hci_proto) < 0)
4754 BT_ERR("L2CAP protocol unregistration failed");
4755
bb58f747 4756 l2cap_cleanup_sockets();
1da177e4
LT		 4757}
4758
d1c4a17d
GP		 4759module_param(disable_ertm, bool, 0644);
4760MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode");
kernel source for telekom puls (alcatel/mediatek)