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