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