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