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