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