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