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