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