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