projects / deliverable / linux.git / blame
? search:
summary | shortlog | log | commit | commitdiff | tree
blob | blame (incremental) | history | HEAD
Bluetooth: Fix L2CAP command reject reason
[deliverable/linux.git] / net / bluetooth / l2cap_core.c
CommitLineData
8e87d142 1/*
1da177e4
LT		 2 BlueZ - Bluetooth protocol stack for Linux
3 Copyright (C) 2000-2001 Qualcomm Incorporated
ce5706bd 4 Copyright (C) 2009-2010 Gustavo F. Padovan <gustavo@padovan.org>
5d8868ff 5 Copyright (C) 2010 Google Inc.
590051de 6 Copyright (C) 2011 ProFUSION Embedded Systems
422e925b 7 Copyright (c) 2012 Code Aurora Forum. All rights reserved.
1da177e4
LT		 8
9 Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.com>
10
11 This program is free software; you can redistribute it and/or modify
12 it under the terms of the GNU General Public License version 2 as
13 published by the Free Software Foundation;
14
15 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
16 OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS.
18 IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY
8e87d142
YH		 19 CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES
20 WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
21 ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
1da177e4
LT		 22 OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
23
8e87d142
YH		 24 ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS,
25 COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS
1da177e4
LT		 26 SOFTWARE IS DISCLAIMED.
27*/
28
bb58f747 29/* Bluetooth L2CAP core. */
1da177e4 30
1da177e4
LT		 31#include <linux/module.h>
32
aef7d97c 33#include <linux/debugfs.h>
fcc203c3 34#include <linux/crc16.h>
1da177e4
LT		 35
36#include <net/bluetooth/bluetooth.h>
37#include <net/bluetooth/hci_core.h>
38#include <net/bluetooth/l2cap.h>
b501d6a1 39#include <net/bluetooth/smp.h>
97e8e89d 40#include <net/bluetooth/a2mp.h>
5ce66b59 41#include <net/bluetooth/amp.h>
1da177e4 42
d1de6d46 43bool disable_ertm;
f0709e03 44
47ec1dcd 45static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN;
50a147cd 46static u8 l2cap_fixed_chan[8] = { L2CAP_FC_L2CAP, };
1da177e4 47
b5ad8b7f
JB		 48static LIST_HEAD(chan_list);
49static DEFINE_RWLOCK(chan_list_lock);
1da177e4 50
1da177e4 51static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
2d792818 52 u8 code, u8 ident, u16 dlen, void *data);
4519de9a 53static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len,
2d792818 54 void *data);
710f9b0a 55static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data);
5e4e3972 56static void l2cap_send_disconn_req(struct l2cap_chan *chan, int err);
1da177e4 57
d660366d 58static void l2cap_tx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
2d792818 59 struct sk_buff_head *skbs, u8 event);
608bcc6d 60
0139418c 61/* ---- L2CAP channels ---- */
71ba0e56 62
2d792818
GP		 63static struct l2cap_chan *__l2cap_get_chan_by_dcid(struct l2cap_conn *conn,
64 u16 cid)
0139418c 65{
3df91ea2 66 struct l2cap_chan *c;
3d57dc68 67
3df91ea2
AE		 68 list_for_each_entry(c, &conn->chan_l, list) {
69 if (c->dcid == cid)
70 return c;
0139418c 71 }
3df91ea2 72 return NULL;
0139418c
MH		 73}
74
2d792818
GP		 75static struct l2cap_chan *__l2cap_get_chan_by_scid(struct l2cap_conn *conn,
76 u16 cid)
0139418c 77{
3df91ea2 78 struct l2cap_chan *c;
3d57dc68 79
3df91ea2
AE		 80 list_for_each_entry(c, &conn->chan_l, list) {
81 if (c->scid == cid)
82 return c;
0139418c 83 }
3df91ea2 84 return NULL;
0139418c
MH		 85}
86
87/* Find channel with given SCID.
ef191ade 88 * Returns locked channel. */
2d792818
GP		 89static struct l2cap_chan *l2cap_get_chan_by_scid(struct l2cap_conn *conn,
90 u16 cid)
0139418c 91{
48454079 92 struct l2cap_chan *c;
baa7e1fa 93
3df91ea2 94 mutex_lock(&conn->chan_lock);
baa7e1fa 95 c = __l2cap_get_chan_by_scid(conn, cid);
ef191ade
MM		 96 if (c)
97 l2cap_chan_lock(c);
3df91ea2
AE		 98 mutex_unlock(&conn->chan_lock);
99
48454079 100 return c;
0139418c
MH		 101}
102
b1a130b7
MM		 103/* Find channel with given DCID.
104 * Returns locked channel.
105 */
106static struct l2cap_chan *l2cap_get_chan_by_dcid(struct l2cap_conn *conn,
107 u16 cid)
108{
109 struct l2cap_chan *c;
110
111 mutex_lock(&conn->chan_lock);
112 c = __l2cap_get_chan_by_dcid(conn, cid);
113 if (c)
114 l2cap_chan_lock(c);
115 mutex_unlock(&conn->chan_lock);
116
117 return c;
118}
119
2d792818
GP		 120static struct l2cap_chan *__l2cap_get_chan_by_ident(struct l2cap_conn *conn,
121 u8 ident)
0139418c 122{
3df91ea2 123 struct l2cap_chan *c;
3d57dc68 124
3df91ea2
AE		 125 list_for_each_entry(c, &conn->chan_l, list) {
126 if (c->ident == ident)
127 return c;
0139418c 128 }
3df91ea2 129 return NULL;
0139418c
MH		 130}
131
5b155ef9
MM		 132static struct l2cap_chan *l2cap_get_chan_by_ident(struct l2cap_conn *conn,
133 u8 ident)
134{
135 struct l2cap_chan *c;
136
137 mutex_lock(&conn->chan_lock);
138 c = __l2cap_get_chan_by_ident(conn, ident);
139 if (c)
140 l2cap_chan_lock(c);
141 mutex_unlock(&conn->chan_lock);
142
143 return c;
144}
145
23691d75 146static struct l2cap_chan *__l2cap_global_chan_by_addr(__le16 psm, bdaddr_t *src)
9e4425ff 147{
23691d75 148 struct l2cap_chan *c;
9e4425ff 149
23691d75
GP		 150 list_for_each_entry(c, &chan_list, global_l) {
151 if (c->sport == psm && !bacmp(&bt_sk(c->sk)->src, src))
250938cb 152 return c;
9e4425ff 153 }
250938cb 154 return NULL;
9e4425ff
GP		 155}
156
157int l2cap_add_psm(struct l2cap_chan *chan, bdaddr_t *src, __le16 psm)
158{
73b2ec18
GP		 159 int err;
160
333055f2 161 write_lock(&chan_list_lock);
9e4425ff 162
23691d75 163 if (psm && __l2cap_global_chan_by_addr(psm, src)) {
73b2ec18
GP		 164 err = -EADDRINUSE;
165 goto done;
9e4425ff
GP		 166 }
167
73b2ec18
GP		 168 if (psm) {
169 chan->psm = psm;
170 chan->sport = psm;
171 err = 0;
172 } else {
173 u16 p;
174
175 err = -EINVAL;
176 for (p = 0x1001; p < 0x1100; p += 2)
23691d75 177 if (!__l2cap_global_chan_by_addr(cpu_to_le16(p), src)) {
73b2ec18
GP		 178 chan->psm = cpu_to_le16(p);
179 chan->sport = cpu_to_le16(p);
180 err = 0;
181 break;
182 }
183 }
9e4425ff 184
73b2ec18 185done:
333055f2 186 write_unlock(&chan_list_lock);
73b2ec18 187 return err;
9e4425ff
GP		 188}
189
190int l2cap_add_scid(struct l2cap_chan *chan, __u16 scid)
191{
333055f2 192 write_lock(&chan_list_lock);
9e4425ff
GP		 193
194 chan->scid = scid;
195
333055f2 196 write_unlock(&chan_list_lock);
9e4425ff
GP		 197
198 return 0;
199}
200
baa7e1fa 201static u16 l2cap_alloc_cid(struct l2cap_conn *conn)
0139418c 202{
8db4dc46 203 u16 cid = L2CAP_CID_DYN_START;
0139418c 204
8db4dc46 205 for (; cid < L2CAP_CID_DYN_END; cid++) {
baa7e1fa 206 if (!__l2cap_get_chan_by_scid(conn, cid))
0139418c
MH		 207 return cid;
208 }
209
210 return 0;
211}
212
0e587be7 213static void __l2cap_state_change(struct l2cap_chan *chan, int state)
89bc500e 214{
42d2d87c 215 BT_DBG("chan %p %s -> %s", chan, state_to_string(chan->state),
2d792818 216 state_to_string(state));
badaaa00 217
89bc500e 218 chan->state = state;
80b98027 219 chan->ops->state_change(chan, state);
89bc500e
GP		 220}
221
0e587be7
AE		 222static void l2cap_state_change(struct l2cap_chan *chan, int state)
223{
224 struct sock *sk = chan->sk;
225
226 lock_sock(sk);
227 __l2cap_state_change(chan, state);
228 release_sock(sk);
229}
230
2e0052e4
AE		 231static inline void __l2cap_chan_set_err(struct l2cap_chan *chan, int err)
232{
233 struct sock *sk = chan->sk;
234
235 sk->sk_err = err;
236}
237
238static inline void l2cap_chan_set_err(struct l2cap_chan *chan, int err)
239{
240 struct sock *sk = chan->sk;
241
242 lock_sock(sk);
243 __l2cap_chan_set_err(chan, err);
244 release_sock(sk);
245}
246
4239d16f
MM		 247static void __set_retrans_timer(struct l2cap_chan *chan)
248{
249 if (!delayed_work_pending(&chan->monitor_timer) &&
250 chan->retrans_timeout) {
251 l2cap_set_timer(chan, &chan->retrans_timer,
252 msecs_to_jiffies(chan->retrans_timeout));
253 }
254}
255
256static void __set_monitor_timer(struct l2cap_chan *chan)
257{
258 __clear_retrans_timer(chan);
259 if (chan->monitor_timeout) {
260 l2cap_set_timer(chan, &chan->monitor_timer,
261 msecs_to_jiffies(chan->monitor_timeout));
262 }
263}
264
608bcc6d
MM		 265static struct sk_buff *l2cap_ertm_seq_in_queue(struct sk_buff_head *head,
266 u16 seq)
267{
268 struct sk_buff *skb;
269
270 skb_queue_walk(head, skb) {
271 if (bt_cb(skb)->control.txseq == seq)
272 return skb;
273 }
274
275 return NULL;
276}
277
3c588192
MM		 278/* ---- L2CAP sequence number lists ---- */
279
280/* For ERTM, ordered lists of sequence numbers must be tracked for
281 * SREJ requests that are received and for frames that are to be
282 * retransmitted. These seq_list functions implement a singly-linked
283 * list in an array, where membership in the list can also be checked
284 * in constant time. Items can also be added to the tail of the list
285 * and removed from the head in constant time, without further memory
286 * allocs or frees.
287 */
288
289static int l2cap_seq_list_init(struct l2cap_seq_list *seq_list, u16 size)
290{
291 size_t alloc_size, i;
292
293 /* Allocated size is a power of 2 to map sequence numbers
294 * (which may be up to 14 bits) in to a smaller array that is
295 * sized for the negotiated ERTM transmit windows.
296 */
297 alloc_size = roundup_pow_of_two(size);
298
299 seq_list->list = kmalloc(sizeof(u16) * alloc_size, GFP_KERNEL);
300 if (!seq_list->list)
301 return -ENOMEM;
302
303 seq_list->mask = alloc_size - 1;
304 seq_list->head = L2CAP_SEQ_LIST_CLEAR;
305 seq_list->tail = L2CAP_SEQ_LIST_CLEAR;
306 for (i = 0; i < alloc_size; i++)
307 seq_list->list[i] = L2CAP_SEQ_LIST_CLEAR;
308
309 return 0;
310}
311
312static inline void l2cap_seq_list_free(struct l2cap_seq_list *seq_list)
313{
314 kfree(seq_list->list);
315}
316
317static inline bool l2cap_seq_list_contains(struct l2cap_seq_list *seq_list,
318 u16 seq)
319{
320 /* Constant-time check for list membership */
321 return seq_list->list[seq & seq_list->mask] != L2CAP_SEQ_LIST_CLEAR;
322}
323
324static u16 l2cap_seq_list_remove(struct l2cap_seq_list *seq_list, u16 seq)
325{
326 u16 mask = seq_list->mask;
327
328 if (seq_list->head == L2CAP_SEQ_LIST_CLEAR) {
329 /* In case someone tries to pop the head of an empty list */
330 return L2CAP_SEQ_LIST_CLEAR;
331 } else if (seq_list->head == seq) {
332 /* Head can be removed in constant time */
333 seq_list->head = seq_list->list[seq & mask];
334 seq_list->list[seq & mask] = L2CAP_SEQ_LIST_CLEAR;
335
336 if (seq_list->head == L2CAP_SEQ_LIST_TAIL) {
337 seq_list->head = L2CAP_SEQ_LIST_CLEAR;
338 seq_list->tail = L2CAP_SEQ_LIST_CLEAR;
339 }
340 } else {
341 /* Walk the list to find the sequence number */
342 u16 prev = seq_list->head;
343 while (seq_list->list[prev & mask] != seq) {
344 prev = seq_list->list[prev & mask];
345 if (prev == L2CAP_SEQ_LIST_TAIL)
346 return L2CAP_SEQ_LIST_CLEAR;
347 }
348
349 /* Unlink the number from the list and clear it */
350 seq_list->list[prev & mask] = seq_list->list[seq & mask];
351 seq_list->list[seq & mask] = L2CAP_SEQ_LIST_CLEAR;
352 if (seq_list->tail == seq)
353 seq_list->tail = prev;
354 }
355 return seq;
356}
357
358static inline u16 l2cap_seq_list_pop(struct l2cap_seq_list *seq_list)
359{
360 /* Remove the head in constant time */
361 return l2cap_seq_list_remove(seq_list, seq_list->head);
362}
363
364static void l2cap_seq_list_clear(struct l2cap_seq_list *seq_list)
365{
f522ae36 366 u16 i;
3c588192 367
f522ae36
GP		 368 if (seq_list->head == L2CAP_SEQ_LIST_CLEAR)
369 return;
370
371 for (i = 0; i <= seq_list->mask; i++)
372 seq_list->list[i] = L2CAP_SEQ_LIST_CLEAR;
373
374 seq_list->head = L2CAP_SEQ_LIST_CLEAR;
375 seq_list->tail = L2CAP_SEQ_LIST_CLEAR;
3c588192
MM		 376}
377
378static void l2cap_seq_list_append(struct l2cap_seq_list *seq_list, u16 seq)
379{
380 u16 mask = seq_list->mask;
381
382 /* All appends happen in constant time */
383
f522ae36
GP		 384 if (seq_list->list[seq & mask] != L2CAP_SEQ_LIST_CLEAR)
385 return;
3c588192 386
f522ae36
GP		 387 if (seq_list->tail == L2CAP_SEQ_LIST_CLEAR)
388 seq_list->head = seq;
389 else
390 seq_list->list[seq_list->tail & mask] = seq;
391
392 seq_list->tail = seq;
393 seq_list->list[seq & mask] = L2CAP_SEQ_LIST_TAIL;
3c588192
MM		 394}
395
721c4181 396static void l2cap_chan_timeout(struct work_struct *work)
ab07801d 397{
721c4181 398 struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
2d792818 399 chan_timer.work);
3df91ea2 400 struct l2cap_conn *conn = chan->conn;
ab07801d
GP		 401 int reason;
402
e05dcc32 403 BT_DBG("chan %p state %s", chan, state_to_string(chan->state));
ab07801d 404
3df91ea2 405 mutex_lock(&conn->chan_lock);
6be36555 406 l2cap_chan_lock(chan);
ab07801d 407
89bc500e 408 if (chan->state == BT_CONNECTED || chan->state == BT_CONFIG)
ab07801d 409 reason = ECONNREFUSED;
89bc500e 410 else if (chan->state == BT_CONNECT &&
2d792818 411 chan->sec_level != BT_SECURITY_SDP)
ab07801d
GP		 412 reason = ECONNREFUSED;
413 else
414 reason = ETIMEDOUT;
415
0f852724 416 l2cap_chan_close(chan, reason);
ab07801d 417
6be36555 418 l2cap_chan_unlock(chan);
ab07801d 419
80b98027 420 chan->ops->close(chan);
3df91ea2
AE		 421 mutex_unlock(&conn->chan_lock);
422
371fd835 423 l2cap_chan_put(chan);
ab07801d
GP		 424}
425
eef1d9b6 426struct l2cap_chan *l2cap_chan_create(void)
48454079
GP		 427{
428 struct l2cap_chan *chan;
429
430 chan = kzalloc(sizeof(*chan), GFP_ATOMIC);
431 if (!chan)
432 return NULL;
433
c03b355e
AE		 434 mutex_init(&chan->lock);
435
333055f2 436 write_lock(&chan_list_lock);
23691d75 437 list_add(&chan->global_l, &chan_list);
333055f2 438 write_unlock(&chan_list_lock);
23691d75 439
721c4181 440 INIT_DELAYED_WORK(&chan->chan_timer, l2cap_chan_timeout);
ab07801d 441
89bc500e
GP		 442 chan->state = BT_OPEN;
443
144ad330 444 kref_init(&chan->kref);
71ba0e56 445
2827011f
MM		 446 /* This flag is cleared in l2cap_chan_ready() */
447 set_bit(CONF_NOT_COMPLETE, &chan->conf_state);
448
eef1d9b6 449 BT_DBG("chan %p", chan);
abc545b8 450
48454079
GP		 451 return chan;
452}
453
144ad330 454static void l2cap_chan_destroy(struct kref *kref)
6ff5abbf 455{
144ad330
SS		 456 struct l2cap_chan *chan = container_of(kref, struct l2cap_chan, kref);
457
4af66c69
JK		 458 BT_DBG("chan %p", chan);
459
333055f2 460 write_lock(&chan_list_lock);
23691d75 461 list_del(&chan->global_l);
333055f2 462 write_unlock(&chan_list_lock);
23691d75 463
4af66c69 464 kfree(chan);
6ff5abbf
GP		 465}
466
30648372
JK		 467void l2cap_chan_hold(struct l2cap_chan *c)
468{
144ad330 469 BT_DBG("chan %p orig refcnt %d", c, atomic_read(&c->kref.refcount));
30648372 470
144ad330 471 kref_get(&c->kref);
30648372
JK		 472}
473
474void l2cap_chan_put(struct l2cap_chan *c)
475{
144ad330 476 BT_DBG("chan %p orig refcnt %d", c, atomic_read(&c->kref.refcount));
30648372 477
144ad330 478 kref_put(&c->kref, l2cap_chan_destroy);
30648372
JK		 479}
480
bd4b1653
AE		 481void l2cap_chan_set_defaults(struct l2cap_chan *chan)
482{
483 chan->fcs = L2CAP_FCS_CRC16;
484 chan->max_tx = L2CAP_DEFAULT_MAX_TX;
485 chan->tx_win = L2CAP_DEFAULT_TX_WINDOW;
486 chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW;
c20f8e35 487 chan->ack_win = L2CAP_DEFAULT_TX_WINDOW;
bd4b1653
AE		 488 chan->sec_level = BT_SECURITY_LOW;
489
490 set_bit(FLAG_FORCE_ACTIVE, &chan->flags);
491}
492
93c3e8f5 493void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
0139418c 494{
af05b30b 495 BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn,
097db76c 496 __le16_to_cpu(chan->psm), chan->dcid);
0139418c 497
9f5a0d7b 498 conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM;
2950f21a 499
8c1d787b 500 chan->conn = conn;
0139418c 501
5491120e
AE		 502 switch (chan->chan_type) {
503 case L2CAP_CHAN_CONN_ORIENTED:
b62f328b
VT		 504 if (conn->hcon->type == LE_LINK) {
505 /* LE connection */
6fcb06a2 506 chan->omtu = L2CAP_DEFAULT_MTU;
9f22398c
JH		 507 if (chan->dcid == L2CAP_CID_ATT)
508 chan->scid = L2CAP_CID_ATT;
509 else
510 chan->scid = l2cap_alloc_cid(conn);
b62f328b
VT		 511 } else {
512 /* Alloc CID for connection-oriented socket */
fe4128e0 513 chan->scid = l2cap_alloc_cid(conn);
0c1bc5c6 514 chan->omtu = L2CAP_DEFAULT_MTU;
b62f328b 515 }
5491120e
AE		 516 break;
517
518 case L2CAP_CHAN_CONN_LESS:
0139418c 519 /* Connectionless socket */
fe4128e0
GP		 520 chan->scid = L2CAP_CID_CONN_LESS;
521 chan->dcid = L2CAP_CID_CONN_LESS;
0c1bc5c6 522 chan->omtu = L2CAP_DEFAULT_MTU;
5491120e
AE		 523 break;
524
416fa752
AE		 525 case L2CAP_CHAN_CONN_FIX_A2MP:
526 chan->scid = L2CAP_CID_A2MP;
527 chan->dcid = L2CAP_CID_A2MP;
528 chan->omtu = L2CAP_A2MP_DEFAULT_MTU;
529 chan->imtu = L2CAP_A2MP_DEFAULT_MTU;
530 break;
531
5491120e 532 default:
0139418c 533 /* Raw socket can send/recv signalling messages only */
fe4128e0
GP		 534 chan->scid = L2CAP_CID_SIGNALING;
535 chan->dcid = L2CAP_CID_SIGNALING;
0c1bc5c6 536 chan->omtu = L2CAP_DEFAULT_MTU;
0139418c
MH		 537 }
538
8f7975b1
AE		 539 chan->local_id = L2CAP_BESTEFFORT_ID;
540 chan->local_stype = L2CAP_SERV_BESTEFFORT;
541 chan->local_msdu = L2CAP_DEFAULT_MAX_SDU_SIZE;
542 chan->local_sdu_itime = L2CAP_DEFAULT_SDU_ITIME;
543 chan->local_acc_lat = L2CAP_DEFAULT_ACC_LAT;
8936fa6d 544 chan->local_flush_to = L2CAP_EFS_DEFAULT_FLUSH_TO;
8f7975b1 545
371fd835 546 l2cap_chan_hold(chan);
baa7e1fa 547
5ee9891d
JH		 548 hci_conn_hold(conn->hcon);
549
3df91ea2 550 list_add(&chan->list, &conn->chan_l);
643162a8
AE		 551}
552
466f8004 553void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
643162a8
AE		 554{
555 mutex_lock(&conn->chan_lock);
556 __l2cap_chan_add(conn, chan);
3df91ea2 557 mutex_unlock(&conn->chan_lock);
0139418c
MH		 558}
559
466f8004 560void l2cap_chan_del(struct l2cap_chan *chan, int err)
0139418c 561{
8c1d787b 562 struct l2cap_conn *conn = chan->conn;
0139418c 563
c9b66675 564 __clear_chan_timer(chan);
0139418c 565
49208c9c 566 BT_DBG("chan %p, conn %p, err %d", chan, conn, err);
0139418c 567
8e87d142 568 if (conn) {
56f60984 569 struct amp_mgr *mgr = conn->hcon->amp_mgr;
baa7e1fa 570 /* Delete from channel list */
3df91ea2 571 list_del(&chan->list);
3d57dc68 572
371fd835 573 l2cap_chan_put(chan);
baa7e1fa 574
8c1d787b 575 chan->conn = NULL;
3cabbfda
AE		 576
577 if (chan->chan_type != L2CAP_CHAN_CONN_FIX_A2MP)
76a68ba0 578 hci_conn_drop(conn->hcon);
56f60984
AE		 579
580 if (mgr && mgr->bredr_chan == chan)
581 mgr->bredr_chan = NULL;
0139418c
MH		 582 }
583
419e08c1
AE		 584 if (chan->hs_hchan) {
585 struct hci_chan *hs_hchan = chan->hs_hchan;
586
587 BT_DBG("chan %p disconnect hs_hchan %p", chan, hs_hchan);
588 amp_disconnect_logical_link(hs_hchan);
589 }
590
b699ec0d 591 chan->ops->teardown(chan, err);
6be36555 592
2827011f 593 if (test_bit(CONF_NOT_COMPLETE, &chan->conf_state))
6ff5abbf 594 return;
2ead70b8 595
ee556f66
GP		 596 switch(chan->mode) {
597 case L2CAP_MODE_BASIC:
598 break;
c13ffa62 599
ee556f66 600 case L2CAP_MODE_ERTM:
1a09bcb9
GP		 601 __clear_retrans_timer(chan);
602 __clear_monitor_timer(chan);
603 __clear_ack_timer(chan);
c13ffa62 604
f1c6775b 605 skb_queue_purge(&chan->srej_q);
c13ffa62 606
3c588192
MM		 607 l2cap_seq_list_free(&chan->srej_list);
608 l2cap_seq_list_free(&chan->retrans_list);
ee556f66
GP		 609
610 /* fall through */
611
612 case L2CAP_MODE_STREAMING:
613 skb_queue_purge(&chan->tx_q);
614 break;
c13ffa62 615 }
ee556f66
GP		 616
617 return;
0139418c
MH		 618}
619
0f852724 620void l2cap_chan_close(struct l2cap_chan *chan, int reason)
4519de9a
GP		 621{
622 struct l2cap_conn *conn = chan->conn;
623 struct sock *sk = chan->sk;
624
2d792818
GP		 625 BT_DBG("chan %p state %s sk %p", chan, state_to_string(chan->state),
626 sk);
4519de9a 627
89bc500e 628 switch (chan->state) {
4519de9a 629 case BT_LISTEN:
b699ec0d 630 chan->ops->teardown(chan, 0);
4519de9a
GP		 631 break;
632
633 case BT_CONNECTED:
634 case BT_CONFIG:
715ec005 635 if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED &&
2d792818 636 conn->hcon->type == ACL_LINK) {
c9b66675 637 __set_chan_timer(chan, sk->sk_sndtimeo);
5e4e3972 638 l2cap_send_disconn_req(chan, reason);
4519de9a
GP		 639 } else
640 l2cap_chan_del(chan, reason);
641 break;
642
643 case BT_CONNECT2:
715ec005 644 if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED &&
2d792818 645 conn->hcon->type == ACL_LINK) {
4519de9a
GP		 646 struct l2cap_conn_rsp rsp;
647 __u16 result;
648
c5daa683 649 if (test_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags))
4519de9a
GP		 650 result = L2CAP_CR_SEC_BLOCK;
651 else
652 result = L2CAP_CR_BAD_PSM;
89bc500e 653 l2cap_state_change(chan, BT_DISCONN);
4519de9a
GP		 654
655 rsp.scid = cpu_to_le16(chan->dcid);
656 rsp.dcid = cpu_to_le16(chan->scid);
657 rsp.result = cpu_to_le16(result);
ac73498c 658 rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
4519de9a 659 l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
2d792818 660 sizeof(rsp), &rsp);
4519de9a
GP		 661 }
662
663 l2cap_chan_del(chan, reason);
664 break;
665
666 case BT_CONNECT:
667 case BT_DISCONN:
668 l2cap_chan_del(chan, reason);
669 break;
670
671 default:
b699ec0d 672 chan->ops->teardown(chan, 0);
4519de9a
GP		 673 break;
674 }
675}
676
4343478f 677static inline u8 l2cap_get_auth_type(struct l2cap_chan *chan)
79d554a6 678{
715ec005 679 if (chan->chan_type == L2CAP_CHAN_RAW) {
4343478f 680 switch (chan->sec_level) {
8556edd3
JH		 681 case BT_SECURITY_HIGH:
682 return HCI_AT_DEDICATED_BONDING_MITM;
683 case BT_SECURITY_MEDIUM:
684 return HCI_AT_DEDICATED_BONDING;
685 default:
686 return HCI_AT_NO_BONDING;
687 }
2983fd68 688 } else if (chan->psm == __constant_cpu_to_le16(L2CAP_PSM_SDP)) {
4343478f
GP		 689 if (chan->sec_level == BT_SECURITY_LOW)
690 chan->sec_level = BT_SECURITY_SDP;
0684e5f9 691
4343478f 692 if (chan->sec_level == BT_SECURITY_HIGH)
8556edd3 693 return HCI_AT_NO_BONDING_MITM;
00ae4af9 694 else
8556edd3 695 return HCI_AT_NO_BONDING;
00ae4af9 696 } else {
4343478f 697 switch (chan->sec_level) {
00ae4af9 698 case BT_SECURITY_HIGH:
8556edd3 699 return HCI_AT_GENERAL_BONDING_MITM;
00ae4af9 700 case BT_SECURITY_MEDIUM:
8556edd3 701 return HCI_AT_GENERAL_BONDING;
00ae4af9 702 default:
8556edd3 703 return HCI_AT_NO_BONDING;
00ae4af9 704 }
0684e5f9 705 }
8556edd3
JH		 706}
707
708/* Service level security */
d45fc423 709int l2cap_chan_check_security(struct l2cap_chan *chan)
8556edd3 710{
8c1d787b 711 struct l2cap_conn *conn = chan->conn;
8556edd3
JH		 712 __u8 auth_type;
713
4343478f 714 auth_type = l2cap_get_auth_type(chan);
79d554a6 715
4343478f 716 return hci_conn_security(conn->hcon, chan->sec_level, auth_type);
79d554a6
MH		 717}
718
b5ad8b7f 719static u8 l2cap_get_ident(struct l2cap_conn *conn)
4e8402a3
MH		 720{
721 u8 id;
722
723 /* Get next available identificator.
724 * 1 - 128 are used by kernel.
725 * 129 - 199 are reserved.
726 * 200 - 254 are used by utilities like l2ping, etc.
727 */
728
333055f2 729 spin_lock(&conn->lock);
4e8402a3
MH		 730
731 if (++conn->tx_ident > 128)
732 conn->tx_ident = 1;
733
734 id = conn->tx_ident;
735
333055f2 736 spin_unlock(&conn->lock);
4e8402a3
MH		 737
738 return id;
739}
740
2d792818
GP		 741static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len,
742 void *data)
4e8402a3
MH		 743{
744 struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data);
e702112f 745 u8 flags;
4e8402a3
MH		 746
747 BT_DBG("code 0x%2.2x", code);
748
749 if (!skb)
9a9c6a34 750 return;
4e8402a3 751
e702112f
AE		 752 if (lmp_no_flush_capable(conn->hcon->hdev))
753 flags = ACL_START_NO_FLUSH;
754 else
755 flags = ACL_START;
756
14b12d0b 757 bt_cb(skb)->force_active = BT_POWER_FORCE_ACTIVE_ON;
5e59b791 758 skb->priority = HCI_PRIO_MAX;
14b12d0b 759
73d80deb
LAD		 760 hci_send_acl(conn->hchan, skb, flags);
761}
762
02b0fbb9
MM		 763static bool __chan_is_moving(struct l2cap_chan *chan)
764{
765 return chan->move_state != L2CAP_MOVE_STABLE &&
766 chan->move_state != L2CAP_MOVE_WAIT_PREPARE;
767}
768
73d80deb
LAD		 769static void l2cap_do_send(struct l2cap_chan *chan, struct sk_buff *skb)
770{
771 struct hci_conn *hcon = chan->conn->hcon;
772 u16 flags;
773
774 BT_DBG("chan %p, skb %p len %d priority %u", chan, skb, skb->len,
2d792818 775 skb->priority);
73d80deb 776
d5f8a75d
MM		 777 if (chan->hs_hcon && !__chan_is_moving(chan)) {
778 if (chan->hs_hchan)
779 hci_send_acl(chan->hs_hchan, skb, ACL_COMPLETE);
780 else
781 kfree_skb(skb);
782
783 return;
784 }
785
73d80deb 786 if (!test_bit(FLAG_FLUSHABLE, &chan->flags) &&
2d792818 787 lmp_no_flush_capable(hcon->hdev))
73d80deb
LAD		 788 flags = ACL_START_NO_FLUSH;
789 else
790 flags = ACL_START;
14b12d0b 791
73d80deb
LAD		 792 bt_cb(skb)->force_active = test_bit(FLAG_FORCE_ACTIVE, &chan->flags);
793 hci_send_acl(chan->conn->hchan, skb, flags);
4e8402a3
MH		 794}
795
b76bbd66
MM		 796static void __unpack_enhanced_control(u16 enh, struct l2cap_ctrl *control)
797{
798 control->reqseq = (enh & L2CAP_CTRL_REQSEQ) >> L2CAP_CTRL_REQSEQ_SHIFT;
799 control->final = (enh & L2CAP_CTRL_FINAL) >> L2CAP_CTRL_FINAL_SHIFT;
800
801 if (enh & L2CAP_CTRL_FRAME_TYPE) {
802 /* S-Frame */
803 control->sframe = 1;
804 control->poll = (enh & L2CAP_CTRL_POLL) >> L2CAP_CTRL_POLL_SHIFT;
805 control->super = (enh & L2CAP_CTRL_SUPERVISE) >> L2CAP_CTRL_SUPER_SHIFT;
806
807 control->sar = 0;
808 control->txseq = 0;
809 } else {
810 /* I-Frame */
811 control->sframe = 0;
812 control->sar = (enh & L2CAP_CTRL_SAR) >> L2CAP_CTRL_SAR_SHIFT;
813 control->txseq = (enh & L2CAP_CTRL_TXSEQ) >> L2CAP_CTRL_TXSEQ_SHIFT;
814
815 control->poll = 0;
816 control->super = 0;
817 }
818}
819
b76bbd66
MM		 820static void __unpack_extended_control(u32 ext, struct l2cap_ctrl *control)
821{
822 control->reqseq = (ext & L2CAP_EXT_CTRL_REQSEQ) >> L2CAP_EXT_CTRL_REQSEQ_SHIFT;
823 control->final = (ext & L2CAP_EXT_CTRL_FINAL) >> L2CAP_EXT_CTRL_FINAL_SHIFT;
824
825 if (ext & L2CAP_EXT_CTRL_FRAME_TYPE) {
826 /* S-Frame */
827 control->sframe = 1;
828 control->poll = (ext & L2CAP_EXT_CTRL_POLL) >> L2CAP_EXT_CTRL_POLL_SHIFT;
829 control->super = (ext & L2CAP_EXT_CTRL_SUPERVISE) >> L2CAP_EXT_CTRL_SUPER_SHIFT;
830
831 control->sar = 0;
832 control->txseq = 0;
833 } else {
834 /* I-Frame */
835 control->sframe = 0;
836 control->sar = (ext & L2CAP_EXT_CTRL_SAR) >> L2CAP_EXT_CTRL_SAR_SHIFT;
837 control->txseq = (ext & L2CAP_EXT_CTRL_TXSEQ) >> L2CAP_EXT_CTRL_TXSEQ_SHIFT;
838
839 control->poll = 0;
840 control->super = 0;
841 }
842}
843
844static inline void __unpack_control(struct l2cap_chan *chan,
845 struct sk_buff *skb)
846{
847 if (test_bit(FLAG_EXT_CTRL, &chan->flags)) {
848 __unpack_extended_control(get_unaligned_le32(skb->data),
849 &bt_cb(skb)->control);
cec8ab6e 850 skb_pull(skb, L2CAP_EXT_CTRL_SIZE);
b76bbd66
MM		 851 } else {
852 __unpack_enhanced_control(get_unaligned_le16(skb->data),
853 &bt_cb(skb)->control);
cec8ab6e 854 skb_pull(skb, L2CAP_ENH_CTRL_SIZE);
b76bbd66
MM		 855 }
856}
857
b5c6aaed
MM		 858static u32 __pack_extended_control(struct l2cap_ctrl *control)
859{
860 u32 packed;
861
862 packed = control->reqseq << L2CAP_EXT_CTRL_REQSEQ_SHIFT;
863 packed |= control->final << L2CAP_EXT_CTRL_FINAL_SHIFT;
864
865 if (control->sframe) {
866 packed |= control->poll << L2CAP_EXT_CTRL_POLL_SHIFT;
867 packed |= control->super << L2CAP_EXT_CTRL_SUPER_SHIFT;
868 packed |= L2CAP_EXT_CTRL_FRAME_TYPE;
869 } else {
870 packed |= control->sar << L2CAP_EXT_CTRL_SAR_SHIFT;
871 packed |= control->txseq << L2CAP_EXT_CTRL_TXSEQ_SHIFT;
872 }
873
874 return packed;
875}
876
877static u16 __pack_enhanced_control(struct l2cap_ctrl *control)
878{
879 u16 packed;
880
881 packed = control->reqseq << L2CAP_CTRL_REQSEQ_SHIFT;
882 packed |= control->final << L2CAP_CTRL_FINAL_SHIFT;
883
884 if (control->sframe) {
885 packed |= control->poll << L2CAP_CTRL_POLL_SHIFT;
886 packed |= control->super << L2CAP_CTRL_SUPER_SHIFT;
887 packed |= L2CAP_CTRL_FRAME_TYPE;
888 } else {
889 packed |= control->sar << L2CAP_CTRL_SAR_SHIFT;
890 packed |= control->txseq << L2CAP_CTRL_TXSEQ_SHIFT;
891 }
892
893 return packed;
894}
895
b76bbd66
MM		 896static inline void __pack_control(struct l2cap_chan *chan,
897 struct l2cap_ctrl *control,
898 struct sk_buff *skb)
899{
900 if (test_bit(FLAG_EXT_CTRL, &chan->flags)) {
901 put_unaligned_le32(__pack_extended_control(control),
902 skb->data + L2CAP_HDR_SIZE);
903 } else {
904 put_unaligned_le16(__pack_enhanced_control(control),
905 skb->data + L2CAP_HDR_SIZE);
906 }
907}
908
ba7aa64f
GP		 909static inline unsigned int __ertm_hdr_size(struct l2cap_chan *chan)
910{
911 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
912 return L2CAP_EXT_HDR_SIZE;
913 else
914 return L2CAP_ENH_HDR_SIZE;
915}
916
a67d7f6f
MM		 917static struct sk_buff *l2cap_create_sframe_pdu(struct l2cap_chan *chan,
918 u32 control)
b5c6aaed
MM		 919{
920 struct sk_buff *skb;
921 struct l2cap_hdr *lh;
ba7aa64f 922 int hlen = __ertm_hdr_size(chan);
b5c6aaed
MM		 923
924 if (chan->fcs == L2CAP_FCS_CRC16)
925 hlen += L2CAP_FCS_SIZE;
926
a67d7f6f 927 skb = bt_skb_alloc(hlen, GFP_KERNEL);
b5c6aaed 928
b5c6aaed 929 if (!skb)
a67d7f6f 930 return ERR_PTR(-ENOMEM);
b5c6aaed
MM		 931
932 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
933 lh->len = cpu_to_le16(hlen - L2CAP_HDR_SIZE);
934 lh->cid = cpu_to_le16(chan->dcid);
935
a67d7f6f
MM		 936 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
937 put_unaligned_le32(control, skb_put(skb, L2CAP_EXT_CTRL_SIZE));
938 else
939 put_unaligned_le16(control, skb_put(skb, L2CAP_ENH_CTRL_SIZE));
b5c6aaed
MM		 940
941 if (chan->fcs == L2CAP_FCS_CRC16) {
a67d7f6f 942 u16 fcs = crc16(0, (u8 *)skb->data, skb->len);
b5c6aaed
MM		 943 put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE));
944 }
945
946 skb->priority = HCI_PRIO_MAX;
a67d7f6f
MM		 947 return skb;
948}
949
950static void l2cap_send_sframe(struct l2cap_chan *chan,
951 struct l2cap_ctrl *control)
952{
953 struct sk_buff *skb;
954 u32 control_field;
955
956 BT_DBG("chan %p, control %p", chan, control);
957
958 if (!control->sframe)
959 return;
960
b99e13ad
MM		 961 if (__chan_is_moving(chan))
962 return;
963
a67d7f6f
MM		 964 if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state) &&
965 !control->poll)
966 control->final = 1;
967
968 if (control->super == L2CAP_SUPER_RR)
969 clear_bit(CONN_RNR_SENT, &chan->conn_state);
970 else if (control->super == L2CAP_SUPER_RNR)
971 set_bit(CONN_RNR_SENT, &chan->conn_state);
972
973 if (control->super != L2CAP_SUPER_SREJ) {
974 chan->last_acked_seq = control->reqseq;
975 __clear_ack_timer(chan);
976 }
977
978 BT_DBG("reqseq %d, final %d, poll %d, super %d", control->reqseq,
979 control->final, control->poll, control->super);
980
981 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
982 control_field = __pack_extended_control(control);
983 else
984 control_field = __pack_enhanced_control(control);
985
986 skb = l2cap_create_sframe_pdu(chan, control_field);
987 if (!IS_ERR(skb))
988 l2cap_do_send(chan, skb);
b5c6aaed
MM		 989}
990
c9e3d5e0 991static void l2cap_send_rr_or_rnr(struct l2cap_chan *chan, bool poll)
b5c6aaed 992{
c9e3d5e0
MM		 993 struct l2cap_ctrl control;
994
995 BT_DBG("chan %p, poll %d", chan, poll);
996
997 memset(&control, 0, sizeof(control));
998 control.sframe = 1;
999 control.poll = poll;
1000
1001 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state))
1002 control.super = L2CAP_SUPER_RNR;
1003 else
1004 control.super = L2CAP_SUPER_RR;
b5c6aaed 1005
c9e3d5e0
MM		 1006 control.reqseq = chan->buffer_seq;
1007 l2cap_send_sframe(chan, &control);
b5c6aaed
MM		 1008}
1009
b4450035 1010static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan)
e501d055 1011{
c1360a1c 1012 return !test_bit(CONF_CONNECT_PEND, &chan->conf_state);
e501d055
AE		 1013}
1014
93c3e8f5
AE		 1015static bool __amp_capable(struct l2cap_chan *chan)
1016{
1017 struct l2cap_conn *conn = chan->conn;
1018
1019 if (enable_hs &&
5d05416e 1020 hci_amp_capable() &&
93c3e8f5
AE		 1021 chan->chan_policy == BT_CHANNEL_POLICY_AMP_PREFERRED &&
1022 conn->fixed_chan_mask & L2CAP_FC_A2MP)
1023 return true;
1024 else
1025 return false;
1026}
1027
5ce66b59
AE		 1028static bool l2cap_check_efs(struct l2cap_chan *chan)
1029{
1030 /* Check EFS parameters */
1031 return true;
1032}
1033
2766be48 1034void l2cap_send_conn_req(struct l2cap_chan *chan)
9b27f350
AE		 1035{
1036 struct l2cap_conn *conn = chan->conn;
1037 struct l2cap_conn_req req;
1038
1039 req.scid = cpu_to_le16(chan->scid);
1040 req.psm = chan->psm;
1041
1042 chan->ident = l2cap_get_ident(conn);
1043
1044 set_bit(CONF_CONNECT_PEND, &chan->conf_state);
1045
1046 l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_REQ, sizeof(req), &req);
1047}
1048
8eb200bd
MM		 1049static void l2cap_send_create_chan_req(struct l2cap_chan *chan, u8 amp_id)
1050{
1051 struct l2cap_create_chan_req req;
1052 req.scid = cpu_to_le16(chan->scid);
1053 req.psm = chan->psm;
1054 req.amp_id = amp_id;
1055
1056 chan->ident = l2cap_get_ident(chan->conn);
1057
1058 l2cap_send_cmd(chan->conn, chan->ident, L2CAP_CREATE_CHAN_REQ,
1059 sizeof(req), &req);
1060}
1061
02b0fbb9
MM		 1062static void l2cap_move_setup(struct l2cap_chan *chan)
1063{
1064 struct sk_buff *skb;
1065
1066 BT_DBG("chan %p", chan);
1067
1068 if (chan->mode != L2CAP_MODE_ERTM)
1069 return;
1070
1071 __clear_retrans_timer(chan);
1072 __clear_monitor_timer(chan);
1073 __clear_ack_timer(chan);
1074
1075 chan->retry_count = 0;
1076 skb_queue_walk(&chan->tx_q, skb) {
1077 if (bt_cb(skb)->control.retries)
1078 bt_cb(skb)->control.retries = 1;
1079 else
1080 break;
1081 }
1082
1083 chan->expected_tx_seq = chan->buffer_seq;
1084
1085 clear_bit(CONN_REJ_ACT, &chan->conn_state);
1086 clear_bit(CONN_SREJ_ACT, &chan->conn_state);
1087 l2cap_seq_list_clear(&chan->retrans_list);
1088 l2cap_seq_list_clear(&chan->srej_list);
1089 skb_queue_purge(&chan->srej_q);
1090
1091 chan->tx_state = L2CAP_TX_STATE_XMIT;
1092 chan->rx_state = L2CAP_RX_STATE_MOVE;
1093
1094 set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
1095}
1096
5f3847a4
MM		 1097static void l2cap_move_done(struct l2cap_chan *chan)
1098{
1099 u8 move_role = chan->move_role;
1100 BT_DBG("chan %p", chan);
1101
1102 chan->move_state = L2CAP_MOVE_STABLE;
1103 chan->move_role = L2CAP_MOVE_ROLE_NONE;
1104
1105 if (chan->mode != L2CAP_MODE_ERTM)
1106 return;
1107
1108 switch (move_role) {
1109 case L2CAP_MOVE_ROLE_INITIATOR:
1110 l2cap_tx(chan, NULL, NULL, L2CAP_EV_EXPLICIT_POLL);
1111 chan->rx_state = L2CAP_RX_STATE_WAIT_F;
1112 break;
1113 case L2CAP_MOVE_ROLE_RESPONDER:
1114 chan->rx_state = L2CAP_RX_STATE_WAIT_P;
1115 break;
1116 }
1117}
1118
9f0caeb1
VCG		 1119static void l2cap_chan_ready(struct l2cap_chan *chan)
1120{
2827011f 1121 /* This clears all conf flags, including CONF_NOT_COMPLETE */
9f0caeb1
VCG		 1122 chan->conf_state = 0;
1123 __clear_chan_timer(chan);
1124
54a59aa2 1125 chan->state = BT_CONNECTED;
9f0caeb1 1126
fd83e2c2 1127 chan->ops->ready(chan);
9f0caeb1
VCG		 1128}
1129
93c3e8f5
AE		 1130static void l2cap_start_connection(struct l2cap_chan *chan)
1131{
1132 if (__amp_capable(chan)) {
1133 BT_DBG("chan %p AMP capable: discover AMPs", chan);
1134 a2mp_discover_amp(chan);
1135 } else {
1136 l2cap_send_conn_req(chan);
1137 }
1138}
1139
fc7f8a7e 1140static void l2cap_do_start(struct l2cap_chan *chan)
79d554a6 1141{
8c1d787b 1142 struct l2cap_conn *conn = chan->conn;
79d554a6 1143
9f0caeb1
VCG		 1144 if (conn->hcon->type == LE_LINK) {
1145 l2cap_chan_ready(chan);
1146 return;
1147 }
1148
79d554a6 1149 if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) {
984947dc
MH		 1150 if (!(conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE))
1151 return;
1152
d45fc423 1153 if (l2cap_chan_check_security(chan) &&
2d792818 1154 __l2cap_no_conn_pending(chan)) {
93c3e8f5
AE		 1155 l2cap_start_connection(chan);
1156 }
79d554a6
MH		 1157 } else {
1158 struct l2cap_info_req req;
ac73498c 1159 req.type = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
79d554a6
MH		 1160
1161 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
1162 conn->info_ident = l2cap_get_ident(conn);
1163
ba13ccd9 1164 schedule_delayed_work(&conn->info_timer, L2CAP_INFO_TIMEOUT);
79d554a6 1165
2d792818
GP		 1166 l2cap_send_cmd(conn, conn->info_ident, L2CAP_INFO_REQ,
1167 sizeof(req), &req);
79d554a6
MH		 1168 }
1169}
1170
cf6c2c0b
GP		 1171static inline int l2cap_mode_supported(__u8 mode, __u32 feat_mask)
1172{
1173 u32 local_feat_mask = l2cap_feat_mask;
d1c4a17d 1174 if (!disable_ertm)
cf6c2c0b
GP		 1175 local_feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING;
1176
1177 switch (mode) {
1178 case L2CAP_MODE_ERTM:
1179 return L2CAP_FEAT_ERTM & feat_mask & local_feat_mask;
1180 case L2CAP_MODE_STREAMING:
1181 return L2CAP_FEAT_STREAMING & feat_mask & local_feat_mask;
1182 default:
1183 return 0x00;
1184 }
1185}
1186
5e4e3972 1187static void l2cap_send_disconn_req(struct l2cap_chan *chan, int err)
22121fc9 1188{
6be36555 1189 struct sock *sk = chan->sk;
5e4e3972 1190 struct l2cap_conn *conn = chan->conn;
22121fc9
GP		 1191 struct l2cap_disconn_req req;
1192
c13ffa62
GP		 1193 if (!conn)
1194 return;
1195
aad3d0e3 1196 if (chan->mode == L2CAP_MODE_ERTM && chan->state == BT_CONNECTED) {
1a09bcb9
GP		 1197 __clear_retrans_timer(chan);
1198 __clear_monitor_timer(chan);
1199 __clear_ack_timer(chan);
c13ffa62
GP		 1200 }
1201
416fa752 1202 if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP) {
d117773c 1203 l2cap_state_change(chan, BT_DISCONN);
416fa752
AE		 1204 return;
1205 }
1206
fe4128e0
GP		 1207 req.dcid = cpu_to_le16(chan->dcid);
1208 req.scid = cpu_to_le16(chan->scid);
2d792818
GP		 1209 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_DISCONN_REQ,
1210 sizeof(req), &req);
c13ffa62 1211
6be36555 1212 lock_sock(sk);
0e587be7 1213 __l2cap_state_change(chan, BT_DISCONN);
2e0052e4 1214 __l2cap_chan_set_err(chan, err);
6be36555 1215 release_sock(sk);
22121fc9
GP		 1216}
1217
1da177e4 1218/* ---- L2CAP connections ---- */
4e8402a3
MH		 1219static void l2cap_conn_start(struct l2cap_conn *conn)
1220{
3df91ea2 1221 struct l2cap_chan *chan, *tmp;
4e8402a3
MH		 1222
1223 BT_DBG("conn %p", conn);
1224
3df91ea2 1225 mutex_lock(&conn->chan_lock);
4e8402a3 1226
3df91ea2 1227 list_for_each_entry_safe(chan, tmp, &conn->chan_l, list) {
48454079 1228 struct sock *sk = chan->sk;
baa7e1fa 1229
6be36555 1230 l2cap_chan_lock(chan);
4e8402a3 1231
715ec005 1232 if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
6be36555 1233 l2cap_chan_unlock(chan);
79d554a6
MH		 1234 continue;
1235 }
1236
89bc500e 1237 if (chan->state == BT_CONNECT) {
d45fc423 1238 if (!l2cap_chan_check_security(chan) ||
2d792818 1239 !__l2cap_no_conn_pending(chan)) {
6be36555 1240 l2cap_chan_unlock(chan);
47731de7
GP		 1241 continue;
1242 }
79d554a6 1243
c1360a1c 1244 if (!l2cap_mode_supported(chan->mode, conn->feat_mask)
2d792818 1245 && test_bit(CONF_STATE2_DEVICE,
c1360a1c 1246 &chan->conf_state)) {
89bc500e 1247 l2cap_chan_close(chan, ECONNRESET);
6be36555 1248 l2cap_chan_unlock(chan);
47731de7 1249 continue;
b1235d79 1250 }
47731de7 1251
93c3e8f5 1252 l2cap_start_connection(chan);
47731de7 1253
89bc500e 1254 } else if (chan->state == BT_CONNECT2) {
79d554a6 1255 struct l2cap_conn_rsp rsp;
e9aeb2dd 1256 char buf[128];
fe4128e0
GP		 1257 rsp.scid = cpu_to_le16(chan->dcid);
1258 rsp.dcid = cpu_to_le16(chan->scid);
79d554a6 1259
d45fc423 1260 if (l2cap_chan_check_security(chan)) {
6be36555 1261 lock_sock(sk);
c5daa683
GP		 1262 if (test_bit(BT_SK_DEFER_SETUP,
1263 &bt_sk(sk)->flags)) {
ac73498c
AE		 1264 rsp.result = __constant_cpu_to_le16(L2CAP_CR_PEND);
1265 rsp.status = __constant_cpu_to_le16(L2CAP_CS_AUTHOR_PEND);
2dc4e510 1266 chan->ops->defer(chan);
f66dc81f
MH		 1267
1268 } else {
0e587be7 1269 __l2cap_state_change(chan, BT_CONFIG);
ac73498c
AE		 1270 rsp.result = __constant_cpu_to_le16(L2CAP_CR_SUCCESS);
1271 rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
f66dc81f 1272 }
6be36555 1273 release_sock(sk);
79d554a6 1274 } else {
ac73498c
AE		 1275 rsp.result = __constant_cpu_to_le16(L2CAP_CR_PEND);
1276 rsp.status = __constant_cpu_to_le16(L2CAP_CS_AUTHEN_PEND);
79d554a6
MH		 1277 }
1278
fc7f8a7e 1279 l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
2d792818 1280 sizeof(rsp), &rsp);
e9aeb2dd 1281
c1360a1c 1282 if (test_bit(CONF_REQ_SENT, &chan->conf_state) ||
2d792818 1283 rsp.result != L2CAP_CR_SUCCESS) {
6be36555 1284 l2cap_chan_unlock(chan);
e9aeb2dd
GP		 1285 continue;
1286 }
1287
c1360a1c 1288 set_bit(CONF_REQ_SENT, &chan->conf_state);
e9aeb2dd 1289 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2d792818 1290 l2cap_build_conf_req(chan, buf), buf);
73ffa904 1291 chan->num_conf_req++;
4e8402a3
MH		 1292 }
1293
6be36555 1294 l2cap_chan_unlock(chan);
4e8402a3
MH		 1295 }
1296
3df91ea2 1297 mutex_unlock(&conn->chan_lock);
4e8402a3
MH		 1298}
1299
c2287681 1300/* Find socket with cid and source/destination bdaddr.
b62f328b
VT		 1301 * Returns closest match, locked.
1302 */
d9b88702 1303static struct l2cap_chan *l2cap_global_chan_by_scid(int state, u16 cid,
c2287681
IY		 1304 bdaddr_t *src,
1305 bdaddr_t *dst)
b62f328b 1306{
23691d75 1307 struct l2cap_chan *c, *c1 = NULL;
b62f328b 1308
23691d75 1309 read_lock(&chan_list_lock);
b62f328b 1310
23691d75
GP		 1311 list_for_each_entry(c, &chan_list, global_l) {
1312 struct sock *sk = c->sk;
fe4128e0 1313
89bc500e 1314 if (state && c->state != state)
b62f328b
VT		 1315 continue;
1316
23691d75 1317 if (c->scid == cid) {
c2287681
IY		 1318 int src_match, dst_match;
1319 int src_any, dst_any;
1320
b62f328b 1321 /* Exact match. */
c2287681
IY		 1322 src_match = !bacmp(&bt_sk(sk)->src, src);
1323 dst_match = !bacmp(&bt_sk(sk)->dst, dst);
1324 if (src_match && dst_match) {
23691d75
GP		 1325 read_unlock(&chan_list_lock);
1326 return c;
1327 }
b62f328b
VT		 1328
1329 /* Closest match */
c2287681
IY		 1330 src_any = !bacmp(&bt_sk(sk)->src, BDADDR_ANY);
1331 dst_any = !bacmp(&bt_sk(sk)->dst, BDADDR_ANY);
1332 if ((src_match && dst_any) || (src_any && dst_match) ||
1333 (src_any && dst_any))
23691d75 1334 c1 = c;
b62f328b
VT		 1335 }
1336 }
280f294f 1337
23691d75 1338 read_unlock(&chan_list_lock);
b62f328b 1339
23691d75 1340 return c1;
b62f328b
VT		 1341}
1342
1343static void l2cap_le_conn_ready(struct l2cap_conn *conn)
1344{
60bac184 1345 struct sock *parent;
23691d75 1346 struct l2cap_chan *chan, *pchan;
b62f328b
VT		 1347
1348 BT_DBG("");
1349
1350 /* Check if we have socket listening on cid */
073d1cf3 1351 pchan = l2cap_global_chan_by_scid(BT_LISTEN, L2CAP_CID_ATT,
c2287681 1352 conn->src, conn->dst);
23691d75 1353 if (!pchan)
b62f328b
VT		 1354 return;
1355
44f3b0fb
JH		 1356 /* Client ATT sockets should override the server one */
1357 if (__l2cap_get_chan_by_dcid(conn, L2CAP_CID_ATT))
1358 return;
1359
23691d75
GP		 1360 parent = pchan->sk;
1361
aa2ac881 1362 lock_sock(parent);
62f3a2cf 1363
80b98027 1364 chan = pchan->ops->new_connection(pchan);
80808e43 1365 if (!chan)
b62f328b
VT		 1366 goto clean;
1367
9f22398c
JH		 1368 chan->dcid = L2CAP_CID_ATT;
1369
60bac184
JH		 1370 bacpy(&bt_sk(chan->sk)->src, conn->src);
1371 bacpy(&bt_sk(chan->sk)->dst, conn->dst);
b62f328b 1372
44f3b0fb 1373 __l2cap_chan_add(conn, chan);
48454079 1374
b62f328b 1375clean:
aa2ac881 1376 release_sock(parent);
b62f328b
VT		 1377}
1378
4e8402a3
MH		 1379static void l2cap_conn_ready(struct l2cap_conn *conn)
1380{
48454079 1381 struct l2cap_chan *chan;
cc110922 1382 struct hci_conn *hcon = conn->hcon;
4e8402a3 1383
79d554a6 1384 BT_DBG("conn %p", conn);
4e8402a3 1385
d8729922
JH		 1386 /* For outgoing pairing which doesn't necessarily have an
1387 * associated socket (e.g. mgmt_pair_device).
1388 */
cc110922
VCG		 1389 if (hcon->out && hcon->type == LE_LINK)
1390 smp_conn_security(hcon, hcon->pending_sec_level);
160dc6ac 1391
3df91ea2 1392 mutex_lock(&conn->chan_lock);
4e8402a3 1393
44f3b0fb
JH		 1394 if (hcon->type == LE_LINK)
1395 l2cap_le_conn_ready(conn);
1396
3df91ea2 1397 list_for_each_entry(chan, &conn->chan_l, list) {
baa7e1fa 1398
6be36555 1399 l2cap_chan_lock(chan);
4e8402a3 1400
416fa752
AE		 1401 if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP) {
1402 l2cap_chan_unlock(chan);
1403 continue;
1404 }
1405
cc110922
VCG		 1406 if (hcon->type == LE_LINK) {
1407 if (smp_conn_security(hcon, chan->sec_level))
cf4cd009 1408 l2cap_chan_ready(chan);
acd7d370 1409
63128451 1410 } else if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
6be36555 1411 struct sock *sk = chan->sk;
c9b66675 1412 __clear_chan_timer(chan);
6be36555 1413 lock_sock(sk);
0e587be7 1414 __l2cap_state_change(chan, BT_CONNECTED);
79d554a6 1415 sk->sk_state_change(sk);
6be36555 1416 release_sock(sk);
b501d6a1 1417
1c244f79 1418 } else if (chan->state == BT_CONNECT) {
fc7f8a7e 1419 l2cap_do_start(chan);
1c244f79 1420 }
4e8402a3 1421
6be36555 1422 l2cap_chan_unlock(chan);
4e8402a3 1423 }
79d554a6 1424
3df91ea2 1425 mutex_unlock(&conn->chan_lock);
4e8402a3
MH		 1426}
1427
1428/* Notify sockets that we cannot guaranty reliability anymore */
1429static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err)
1430{
48454079 1431 struct l2cap_chan *chan;
4e8402a3
MH		 1432
1433 BT_DBG("conn %p", conn);
1434
3df91ea2 1435 mutex_lock(&conn->chan_lock);
4e8402a3 1436
3df91ea2 1437 list_for_each_entry(chan, &conn->chan_l, list) {
ecf61bdb 1438 if (test_bit(FLAG_FORCE_RELIABLE, &chan->flags))
1d8b1fd5 1439 l2cap_chan_set_err(chan, err);
4e8402a3
MH		 1440 }
1441
3df91ea2 1442 mutex_unlock(&conn->chan_lock);
4e8402a3
MH		 1443}
1444
f878fcad 1445static void l2cap_info_timeout(struct work_struct *work)
4e8402a3 1446{
f878fcad 1447 struct l2cap_conn *conn = container_of(work, struct l2cap_conn,
2d792818 1448 info_timer.work);
4e8402a3 1449
984947dc 1450 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
e1027a7c 1451 conn->info_ident = 0;
984947dc 1452
4e8402a3
MH		 1453 l2cap_conn_start(conn);
1454}
1455
2c8e1411
DH		 1456/*
1457 * l2cap_user
1458 * External modules can register l2cap_user objects on l2cap_conn. The ->probe
1459 * callback is called during registration. The ->remove callback is called
1460 * during unregistration.
1461 * An l2cap_user object can either be explicitly unregistered or when the
1462 * underlying l2cap_conn object is deleted. This guarantees that l2cap->hcon,
1463 * l2cap->hchan, .. are valid as long as the remove callback hasn't been called.
1464 * External modules must own a reference to the l2cap_conn object if they intend
1465 * to call l2cap_unregister_user(). The l2cap_conn object might get destroyed at
1466 * any time if they don't.
1467 */
1468
1469int l2cap_register_user(struct l2cap_conn *conn, struct l2cap_user *user)
1470{
1471 struct hci_dev *hdev = conn->hcon->hdev;
1472 int ret;
1473
1474 /* We need to check whether l2cap_conn is registered. If it is not, we
1475 * must not register the l2cap_user. l2cap_conn_del() is unregisters
1476 * l2cap_conn objects, but doesn't provide its own locking. Instead, it
1477 * relies on the parent hci_conn object to be locked. This itself relies
1478 * on the hci_dev object to be locked. So we must lock the hci device
1479 * here, too. */
1480
1481 hci_dev_lock(hdev);
1482
1483 if (user->list.next || user->list.prev) {
1484 ret = -EINVAL;
1485 goto out_unlock;
1486 }
1487
1488 /* conn->hchan is NULL after l2cap_conn_del() was called */
1489 if (!conn->hchan) {
1490 ret = -ENODEV;
1491 goto out_unlock;
1492 }
1493
1494 ret = user->probe(conn, user);
1495 if (ret)
1496 goto out_unlock;
1497
1498 list_add(&user->list, &conn->users);
1499 ret = 0;
1500
1501out_unlock:
1502 hci_dev_unlock(hdev);
1503 return ret;
1504}
1505EXPORT_SYMBOL(l2cap_register_user);
1506
1507void l2cap_unregister_user(struct l2cap_conn *conn, struct l2cap_user *user)
1508{
1509 struct hci_dev *hdev = conn->hcon->hdev;
1510
1511 hci_dev_lock(hdev);
1512
1513 if (!user->list.next || !user->list.prev)
1514 goto out_unlock;
1515
1516 list_del(&user->list);
1517 user->list.next = NULL;
1518 user->list.prev = NULL;
1519 user->remove(conn, user);
1520
1521out_unlock:
1522 hci_dev_unlock(hdev);
1523}
1524EXPORT_SYMBOL(l2cap_unregister_user);
1525
1526static void l2cap_unregister_all_users(struct l2cap_conn *conn)
1527{
1528 struct l2cap_user *user;
1529
1530 while (!list_empty(&conn->users)) {
1531 user = list_first_entry(&conn->users, struct l2cap_user, list);
1532 list_del(&user->list);
1533 user->list.next = NULL;
1534 user->list.prev = NULL;
1535 user->remove(conn, user);
1536 }
1537}
1538
5d3de7df
VCG		 1539static void l2cap_conn_del(struct hci_conn *hcon, int err)
1540{
1541 struct l2cap_conn *conn = hcon->l2cap_data;
1542 struct l2cap_chan *chan, *l;
5d3de7df
VCG		 1543
1544 if (!conn)
1545 return;
1546
1547 BT_DBG("hcon %p conn %p, err %d", hcon, conn, err);
1548
1549 kfree_skb(conn->rx_skb);
1550
2c8e1411
DH		 1551 l2cap_unregister_all_users(conn);
1552
3df91ea2
AE		 1553 mutex_lock(&conn->chan_lock);
1554
5d3de7df
VCG		 1555 /* Kill channels */
1556 list_for_each_entry_safe(chan, l, &conn->chan_l, list) {
61d6ef3e 1557 l2cap_chan_hold(chan);
6be36555
AE		 1558 l2cap_chan_lock(chan);
1559
5d3de7df 1560 l2cap_chan_del(chan, err);
6be36555
AE		 1561
1562 l2cap_chan_unlock(chan);
1563
80b98027 1564 chan->ops->close(chan);
61d6ef3e 1565 l2cap_chan_put(chan);
5d3de7df
VCG		 1566 }
1567
3df91ea2
AE		 1568 mutex_unlock(&conn->chan_lock);
1569
73d80deb
LAD		 1570 hci_chan_del(conn->hchan);
1571
5d3de7df 1572 if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT)
127074bf 1573 cancel_delayed_work_sync(&conn->info_timer);
5d3de7df 1574
51a8efd7 1575 if (test_and_clear_bit(HCI_CONN_LE_SMP_PEND, &hcon->flags)) {
127074bf 1576 cancel_delayed_work_sync(&conn->security_timer);
8aab4757 1577 smp_chan_destroy(conn);
d26a2345 1578 }
5d3de7df
VCG		 1579
1580 hcon->l2cap_data = NULL;
9c903e37
DH		 1581 conn->hchan = NULL;
1582 l2cap_conn_put(conn);
5d3de7df
VCG		 1583}
1584
6c9d42a1 1585static void security_timeout(struct work_struct *work)
5d3de7df 1586{
6c9d42a1 1587 struct l2cap_conn *conn = container_of(work, struct l2cap_conn,
2d792818 1588 security_timer.work);
5d3de7df 1589
d06cc416
JH		 1590 BT_DBG("conn %p", conn);
1591
1592 if (test_and_clear_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->flags)) {
1593 smp_chan_destroy(conn);
1594 l2cap_conn_del(conn->hcon, ETIMEDOUT);
1595 }
5d3de7df
VCG		 1596}
1597
baf43251 1598static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon)
1da177e4 1599{
0139418c 1600 struct l2cap_conn *conn = hcon->l2cap_data;
73d80deb 1601 struct hci_chan *hchan;
1da177e4 1602
baf43251 1603 if (conn)
1da177e4
LT		 1604 return conn;
1605
73d80deb
LAD		 1606 hchan = hci_chan_create(hcon);
1607 if (!hchan)
1608 return NULL;
1609
8bcde1f2 1610 conn = kzalloc(sizeof(struct l2cap_conn), GFP_KERNEL);
73d80deb
LAD		 1611 if (!conn) {
1612 hci_chan_del(hchan);
1da177e4 1613 return NULL;
73d80deb 1614 }
1da177e4 1615
9c903e37 1616 kref_init(&conn->ref);
1da177e4
LT		 1617 hcon->l2cap_data = conn;
1618 conn->hcon = hcon;
9c903e37 1619 hci_conn_get(conn->hcon);
73d80deb 1620 conn->hchan = hchan;
1da177e4 1621
73d80deb 1622 BT_DBG("hcon %p conn %p hchan %p", hcon, conn, hchan);
0139418c 1623
dcc042d5 1624 switch (hcon->type) {
dcc042d5
AE		 1625 case LE_LINK:
1626 if (hcon->hdev->le_mtu) {
1627 conn->mtu = hcon->hdev->le_mtu;
1628 break;
1629 }
1630 /* fall through */
dcc042d5 1631 default:
acd7d370 1632 conn->mtu = hcon->hdev->acl_mtu;
dcc042d5
AE		 1633 break;
1634 }
acd7d370 1635
1da177e4
LT		 1636 conn->src = &hcon->hdev->bdaddr;
1637 conn->dst = &hcon->dst;
1638
4e8402a3
MH		 1639 conn->feat_mask = 0;
1640
1da177e4 1641 spin_lock_init(&conn->lock);
3df91ea2 1642 mutex_init(&conn->chan_lock);
baa7e1fa
GP		 1643
1644 INIT_LIST_HEAD(&conn->chan_l);
2c8e1411 1645 INIT_LIST_HEAD(&conn->users);
1da177e4 1646
5d3de7df 1647 if (hcon->type == LE_LINK)
6c9d42a1 1648 INIT_DELAYED_WORK(&conn->security_timer, security_timeout);
5d3de7df 1649 else
030013d8 1650 INIT_DELAYED_WORK(&conn->info_timer, l2cap_info_timeout);
45054dc1 1651
9f5a0d7b 1652 conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM;
2950f21a 1653
1da177e4
LT		 1654 return conn;
1655}
1656
9c903e37
DH		 1657static void l2cap_conn_free(struct kref *ref)
1658{
1659 struct l2cap_conn *conn = container_of(ref, struct l2cap_conn, ref);
1660
1661 hci_conn_put(conn->hcon);
1662 kfree(conn);
1663}
1664
1665void l2cap_conn_get(struct l2cap_conn *conn)
1666{
1667 kref_get(&conn->ref);
1668}
1669EXPORT_SYMBOL(l2cap_conn_get);
1670
1671void l2cap_conn_put(struct l2cap_conn *conn)
1672{
1673 kref_put(&conn->ref, l2cap_conn_free);
1674}
1675EXPORT_SYMBOL(l2cap_conn_put);
1676
1da177e4 1677/* ---- Socket interface ---- */
1da177e4 1678
c2287681 1679/* Find socket with psm and source / destination bdaddr.
1da177e4
LT		 1680 * Returns closest match.
1681 */
c2287681
IY		 1682static struct l2cap_chan *l2cap_global_chan_by_psm(int state, __le16 psm,
1683 bdaddr_t *src,
1684 bdaddr_t *dst)
1da177e4 1685{
23691d75 1686 struct l2cap_chan *c, *c1 = NULL;
1da177e4 1687
23691d75 1688 read_lock(&chan_list_lock);
e0f0cb56 1689
23691d75
GP		 1690 list_for_each_entry(c, &chan_list, global_l) {
1691 struct sock *sk = c->sk;
fe4128e0 1692
89bc500e 1693 if (state && c->state != state)
1da177e4
LT		 1694 continue;
1695
23691d75 1696 if (c->psm == psm) {
c2287681
IY		 1697 int src_match, dst_match;
1698 int src_any, dst_any;
1699
1da177e4 1700 /* Exact match. */
c2287681
IY		 1701 src_match = !bacmp(&bt_sk(sk)->src, src);
1702 dst_match = !bacmp(&bt_sk(sk)->dst, dst);
1703 if (src_match && dst_match) {
a7567b20 1704 read_unlock(&chan_list_lock);
23691d75
GP		 1705 return c;
1706 }
1da177e4
LT		 1707
1708 /* Closest match */
c2287681
IY		 1709 src_any = !bacmp(&bt_sk(sk)->src, BDADDR_ANY);
1710 dst_any = !bacmp(&bt_sk(sk)->dst, BDADDR_ANY);
1711 if ((src_match && dst_any) || (src_any && dst_match) ||
1712 (src_any && dst_any))
23691d75 1713 c1 = c;
1da177e4
LT		 1714 }
1715 }
1da177e4 1716
23691d75 1717 read_unlock(&chan_list_lock);
e0f0cb56 1718
23691d75 1719 return c1;
1da177e4
LT		 1720}
1721
8e9f9892
AG		 1722int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid,
1723 bdaddr_t *dst, u8 dst_type)
1da177e4 1724{
5d41ce1d 1725 struct sock *sk = chan->sk;
1da177e4 1726 bdaddr_t *src = &bt_sk(sk)->src;
1da177e4
LT		 1727 struct l2cap_conn *conn;
1728 struct hci_conn *hcon;
1729 struct hci_dev *hdev;
09ab6f4c 1730 __u8 auth_type;
44d0e48e 1731 int err;
1da177e4 1732
6ed93dc6 1733 BT_DBG("%pMR -> %pMR (type %u) psm 0x%2.2x", src, dst,
ab19516a 1734 dst_type, __le16_to_cpu(psm));
1da177e4 1735
af05b30b
GP		 1736 hdev = hci_get_route(dst, src);
1737 if (!hdev)
1da177e4
LT		 1738 return -EHOSTUNREACH;
1739
09fd0de5 1740 hci_dev_lock(hdev);
1da177e4 1741
6be36555 1742 l2cap_chan_lock(chan);
03a00194
GP		 1743
1744 /* PSM must be odd and lsb of upper byte must be 0 */
1745 if ((__le16_to_cpu(psm) & 0x0101) != 0x0001 && !cid &&
2d792818 1746 chan->chan_type != L2CAP_CHAN_RAW) {
03a00194
GP		 1747 err = -EINVAL;
1748 goto done;
1749 }
1750
1751 if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED && !(psm || cid)) {
1752 err = -EINVAL;
1753 goto done;
1754 }
1755
1756 switch (chan->mode) {
1757 case L2CAP_MODE_BASIC:
1758 break;
1759 case L2CAP_MODE_ERTM:
1760 case L2CAP_MODE_STREAMING:
1761 if (!disable_ertm)
1762 break;
1763 /* fall through */
1764 default:
1765 err = -ENOTSUPP;
1766 goto done;
1767 }
1768
0797e01d 1769 switch (chan->state) {
03a00194
GP		 1770 case BT_CONNECT:
1771 case BT_CONNECT2:
1772 case BT_CONFIG:
1773 /* Already connecting */
1774 err = 0;
1775 goto done;
1776
1777 case BT_CONNECTED:
1778 /* Already connected */
1779 err = -EISCONN;
1780 goto done;
1781
1782 case BT_OPEN:
1783 case BT_BOUND:
1784 /* Can connect */
1785 break;
1786
1787 default:
1788 err = -EBADFD;
1789 goto done;
1790 }
1791
1792 /* Set destination address and psm */
0797e01d 1793 lock_sock(sk);
9219b2a0 1794 bacpy(&bt_sk(sk)->dst, dst);
6be36555
AE		 1795 release_sock(sk);
1796
03a00194
GP		 1797 chan->psm = psm;
1798 chan->dcid = cid;
1da177e4 1799
4343478f 1800 auth_type = l2cap_get_auth_type(chan);
09ab6f4c 1801
f224ca5f 1802 if (bdaddr_type_is_le(dst_type))
8e9f9892 1803 hcon = hci_connect(hdev, LE_LINK, dst, dst_type,
b12f62cf 1804 chan->sec_level, auth_type);
acd7d370 1805 else
8e9f9892 1806 hcon = hci_connect(hdev, ACL_LINK, dst, dst_type,
b12f62cf 1807 chan->sec_level, auth_type);
acd7d370 1808
30e76272
VT		 1809 if (IS_ERR(hcon)) {
1810 err = PTR_ERR(hcon);
1da177e4 1811 goto done;
30e76272 1812 }
1da177e4 1813
baf43251 1814 conn = l2cap_conn_add(hcon);
1da177e4 1815 if (!conn) {
76a68ba0 1816 hci_conn_drop(hcon);
30e76272 1817 err = -ENOMEM;
1da177e4
LT		 1818 goto done;
1819 }
1820
141d5706
JH		 1821 if (cid && __l2cap_get_chan_by_dcid(conn, cid)) {
1822 hci_conn_drop(hcon);
1823 err = -EBUSY;
1824 goto done;
9f0caeb1
VCG		 1825 }
1826
1da177e4
LT		 1827 /* Update source addr of the socket */
1828 bacpy(src, conn->src);
1829
6be36555 1830 l2cap_chan_unlock(chan);
48454079 1831 l2cap_chan_add(conn, chan);
6be36555 1832 l2cap_chan_lock(chan);
48454079 1833
5ee9891d
JH		 1834 /* l2cap_chan_add takes its own ref so we can drop this one */
1835 hci_conn_drop(hcon);
1836
6be36555 1837 l2cap_state_change(chan, BT_CONNECT);
c9b66675 1838 __set_chan_timer(chan, sk->sk_sndtimeo);
1da177e4
LT		 1839
1840 if (hcon->state == BT_CONNECTED) {
715ec005 1841 if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
c9b66675 1842 __clear_chan_timer(chan);
d45fc423 1843 if (l2cap_chan_check_security(chan))
6be36555 1844 l2cap_state_change(chan, BT_CONNECTED);
79d554a6 1845 } else
fc7f8a7e 1846 l2cap_do_start(chan);
1da177e4
LT		 1847 }
1848
30e76272
VT		 1849 err = 0;
1850
1da177e4 1851done:
6be36555 1852 l2cap_chan_unlock(chan);
09fd0de5 1853 hci_dev_unlock(hdev);
1da177e4
LT		 1854 hci_dev_put(hdev);
1855 return err;
1856}
1857
dcba0dba 1858int __l2cap_wait_ack(struct sock *sk)
6161c038 1859{
8c1d787b 1860 struct l2cap_chan *chan = l2cap_pi(sk)->chan;
6161c038
GP		 1861 DECLARE_WAITQUEUE(wait, current);
1862 int err = 0;
1863 int timeo = HZ/5;
1864
2b0b05dd 1865 add_wait_queue(sk_sleep(sk), &wait);
a71a0cf4
PH		 1866 set_current_state(TASK_INTERRUPTIBLE);
1867 while (chan->unacked_frames > 0 && chan->conn) {
6161c038
GP		 1868 if (!timeo)
1869 timeo = HZ/5;
1870
1871 if (signal_pending(current)) {
1872 err = sock_intr_errno(timeo);
1873 break;
1874 }
1875
1876 release_sock(sk);
1877 timeo = schedule_timeout(timeo);
1878 lock_sock(sk);
a71a0cf4 1879 set_current_state(TASK_INTERRUPTIBLE);
6161c038
GP		 1880
1881 err = sock_error(sk);
1882 if (err)
1883 break;
1884 }
1885 set_current_state(TASK_RUNNING);
2b0b05dd 1886 remove_wait_queue(sk_sleep(sk), &wait);
6161c038
GP		 1887 return err;
1888}
1889
721c4181 1890static void l2cap_monitor_timeout(struct work_struct *work)
e90bac06 1891{
721c4181 1892 struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
4239d16f 1893 monitor_timer.work);
e90bac06 1894
525cd185 1895 BT_DBG("chan %p", chan);
0e98958d 1896
6be36555
AE		 1897 l2cap_chan_lock(chan);
1898
80909e04 1899 if (!chan->conn) {
6be36555 1900 l2cap_chan_unlock(chan);
8d7e1c7f 1901 l2cap_chan_put(chan);
e90bac06
GP		 1902 return;
1903 }
1904
401bb1f7 1905 l2cap_tx(chan, NULL, NULL, L2CAP_EV_MONITOR_TO);
e90bac06 1906
6be36555 1907 l2cap_chan_unlock(chan);
8d7e1c7f 1908 l2cap_chan_put(chan);
e90bac06
GP		 1909}
1910
721c4181 1911static void l2cap_retrans_timeout(struct work_struct *work)
e90bac06 1912{
721c4181 1913 struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
4239d16f 1914 retrans_timer.work);
e90bac06 1915
49208c9c 1916 BT_DBG("chan %p", chan);
0e98958d 1917
6be36555
AE		 1918 l2cap_chan_lock(chan);
1919
80909e04
MM		 1920 if (!chan->conn) {
1921 l2cap_chan_unlock(chan);
1922 l2cap_chan_put(chan);
1923 return;
1924 }
6be36555 1925
401bb1f7 1926 l2cap_tx(chan, NULL, NULL, L2CAP_EV_RETRANS_TO);
6be36555 1927 l2cap_chan_unlock(chan);
8d7e1c7f 1928 l2cap_chan_put(chan);
e90bac06
GP		 1929}
1930
d660366d
GP		 1931static void l2cap_streaming_send(struct l2cap_chan *chan,
1932 struct sk_buff_head *skbs)
6840ed07 1933{
ccbb84af 1934 struct sk_buff *skb;
3733937d 1935 struct l2cap_ctrl *control;
6840ed07 1936
3733937d
MM		 1937 BT_DBG("chan %p, skbs %p", chan, skbs);
1938
b99e13ad
MM		 1939 if (__chan_is_moving(chan))
1940 return;
1941
3733937d
MM		 1942 skb_queue_splice_tail_init(skbs, &chan->tx_q);
1943
1944 while (!skb_queue_empty(&chan->tx_q)) {
1945
1946 skb = skb_dequeue(&chan->tx_q);
1947
1948 bt_cb(skb)->control.retries = 1;
1949 control = &bt_cb(skb)->control;
1950
1951 control->reqseq = 0;
1952 control->txseq = chan->next_tx_seq;
1953
1954 __pack_control(chan, control, skb);
6840ed07 1955
47d1ec61 1956 if (chan->fcs == L2CAP_FCS_CRC16) {
3733937d
MM		 1957 u16 fcs = crc16(0, (u8 *) skb->data, skb->len);
1958 put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE));
fcc203c3
GP		 1959 }
1960
4343478f 1961 l2cap_do_send(chan, skb);
6840ed07 1962
b4400672 1963 BT_DBG("Sent txseq %u", control->txseq);
3733937d 1964
836be934 1965 chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq);
3733937d 1966 chan->frames_sent++;
6840ed07 1967 }
6840ed07
GP		 1968}
1969
67c9e840 1970static int l2cap_ertm_send(struct l2cap_chan *chan)
1c2acffb
GP		 1971{
1972 struct sk_buff *skb, *tx_skb;
18a48e76
MM		 1973 struct l2cap_ctrl *control;
1974 int sent = 0;
1975
1976 BT_DBG("chan %p", chan);
1c2acffb 1977
89bc500e 1978 if (chan->state != BT_CONNECTED)
c13ffa62 1979 return -ENOTCONN;
e90bac06 1980
94122bbe
MM		 1981 if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
1982 return 0;
1983
b99e13ad
MM		 1984 if (__chan_is_moving(chan))
1985 return 0;
1986
18a48e76
MM		 1987 while (chan->tx_send_head &&
1988 chan->unacked_frames < chan->remote_tx_win &&
1989 chan->tx_state == L2CAP_TX_STATE_XMIT) {
e420aba3 1990
18a48e76 1991 skb = chan->tx_send_head;
e90bac06 1992
18a48e76
MM		 1993 bt_cb(skb)->control.retries = 1;
1994 control = &bt_cb(skb)->control;
95ffa978 1995
e2ab4353 1996 if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
18a48e76 1997 control->final = 1;
e2ab4353 1998
18a48e76
MM		 1999 control->reqseq = chan->buffer_seq;
2000 chan->last_acked_seq = chan->buffer_seq;
2001 control->txseq = chan->next_tx_seq;
1c2acffb 2002
18a48e76 2003 __pack_control(chan, control, skb);
e90bac06 2004
47d1ec61 2005 if (chan->fcs == L2CAP_FCS_CRC16) {
18a48e76
MM		 2006 u16 fcs = crc16(0, (u8 *) skb->data, skb->len);
2007 put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE));
fcc203c3
GP		 2008 }
2009
18a48e76
MM		 2010 /* Clone after data has been modified. Data is assumed to be
2011 read-only (for locking purposes) on cloned sk_buffs.
2012 */
2013 tx_skb = skb_clone(skb, GFP_KERNEL);
9a9c6a34 2014
18a48e76
MM		 2015 if (!tx_skb)
2016 break;
1c2acffb 2017
18a48e76 2018 __set_retrans_timer(chan);
836be934
AE		 2019
2020 chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq);
18a48e76 2021 chan->unacked_frames++;
6a026610 2022 chan->frames_sent++;
18a48e76 2023 sent++;
1c2acffb 2024
58d35f87
GP		 2025 if (skb_queue_is_last(&chan->tx_q, skb))
2026 chan->tx_send_head = NULL;
1c2acffb 2027 else
58d35f87 2028 chan->tx_send_head = skb_queue_next(&chan->tx_q, skb);
18a48e76
MM		 2029
2030 l2cap_do_send(chan, tx_skb);
b4400672 2031 BT_DBG("Sent txseq %u", control->txseq);
1c2acffb
GP		 2032 }
2033
b4400672
AE		 2034 BT_DBG("Sent %d, %u unacked, %u in ERTM queue", sent,
2035 chan->unacked_frames, skb_queue_len(&chan->tx_q));
18a48e76
MM		 2036
2037 return sent;
9e917af1
GP		 2038}
2039
e1fbd4c1
MM		 2040static void l2cap_ertm_resend(struct l2cap_chan *chan)
2041{
2042 struct l2cap_ctrl control;
2043 struct sk_buff *skb;
2044 struct sk_buff *tx_skb;
2045 u16 seq;
2046
2047 BT_DBG("chan %p", chan);
2048
2049 if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
2050 return;
2051
b99e13ad
MM		 2052 if (__chan_is_moving(chan))
2053 return;
2054
e1fbd4c1
MM		 2055 while (chan->retrans_list.head != L2CAP_SEQ_LIST_CLEAR) {
2056 seq = l2cap_seq_list_pop(&chan->retrans_list);
2057
2058 skb = l2cap_ertm_seq_in_queue(&chan->tx_q, seq);
2059 if (!skb) {
2060 BT_DBG("Error: Can't retransmit seq %d, frame missing",
2d792818 2061 seq);
e1fbd4c1
MM		 2062 continue;
2063 }
2064
2065 bt_cb(skb)->control.retries++;
2066 control = bt_cb(skb)->control;
2067
2068 if (chan->max_tx != 0 &&
2069 bt_cb(skb)->control.retries > chan->max_tx) {
2070 BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
5e4e3972 2071 l2cap_send_disconn_req(chan, ECONNRESET);
e1fbd4c1
MM		 2072 l2cap_seq_list_clear(&chan->retrans_list);
2073 break;
2074 }
2075
2076 control.reqseq = chan->buffer_seq;
2077 if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
2078 control.final = 1;
2079 else
2080 control.final = 0;
2081
2082 if (skb_cloned(skb)) {
2083 /* Cloned sk_buffs are read-only, so we need a
2084 * writeable copy
2085 */
8bcde1f2 2086 tx_skb = skb_copy(skb, GFP_KERNEL);
e1fbd4c1 2087 } else {
8bcde1f2 2088 tx_skb = skb_clone(skb, GFP_KERNEL);
e1fbd4c1
MM		 2089 }
2090
2091 if (!tx_skb) {
2092 l2cap_seq_list_clear(&chan->retrans_list);
2093 break;
2094 }
2095
2096 /* Update skb contents */
2097 if (test_bit(FLAG_EXT_CTRL, &chan->flags)) {
2098 put_unaligned_le32(__pack_extended_control(&control),
2099 tx_skb->data + L2CAP_HDR_SIZE);
2100 } else {
2101 put_unaligned_le16(__pack_enhanced_control(&control),
2102 tx_skb->data + L2CAP_HDR_SIZE);
2103 }
2104
2105 if (chan->fcs == L2CAP_FCS_CRC16) {
2106 u16 fcs = crc16(0, (u8 *) tx_skb->data, tx_skb->len);
2107 put_unaligned_le16(fcs, skb_put(tx_skb,
2108 L2CAP_FCS_SIZE));
2109 }
2110
2111 l2cap_do_send(chan, tx_skb);
2112
2113 BT_DBG("Resent txseq %d", control.txseq);
2114
2115 chan->last_acked_seq = chan->buffer_seq;
2116 }
2117}
2118
f80842a8
MM		 2119static void l2cap_retransmit(struct l2cap_chan *chan,
2120 struct l2cap_ctrl *control)
2121{
2122 BT_DBG("chan %p, control %p", chan, control);
2123
2124 l2cap_seq_list_append(&chan->retrans_list, control->reqseq);
2125 l2cap_ertm_resend(chan);
2126}
2127
d2a7ac5d
MM		 2128static void l2cap_retransmit_all(struct l2cap_chan *chan,
2129 struct l2cap_ctrl *control)
2130{
e1fbd4c1
MM		 2131 struct sk_buff *skb;
2132
2133 BT_DBG("chan %p, control %p", chan, control);
2134
2135 if (control->poll)
2136 set_bit(CONN_SEND_FBIT, &chan->conn_state);
2137
2138 l2cap_seq_list_clear(&chan->retrans_list);
2139
2140 if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
2141 return;
2142
2143 if (chan->unacked_frames) {
2144 skb_queue_walk(&chan->tx_q, skb) {
2145 if (bt_cb(skb)->control.txseq == control->reqseq ||
2d792818 2146 skb == chan->tx_send_head)
e1fbd4c1
MM		 2147 break;
2148 }
2149
2150 skb_queue_walk_from(&chan->tx_q, skb) {
2151 if (skb == chan->tx_send_head)
2152 break;
2153
2154 l2cap_seq_list_append(&chan->retrans_list,
2155 bt_cb(skb)->control.txseq);
2156 }
2157
2158 l2cap_ertm_resend(chan);
2159 }
d2a7ac5d
MM		 2160}
2161
0a0aba42 2162static void l2cap_send_ack(struct l2cap_chan *chan)
9e917af1 2163{
0a0aba42
MM		 2164 struct l2cap_ctrl control;
2165 u16 frames_to_ack = __seq_offset(chan, chan->buffer_seq,
2166 chan->last_acked_seq);
2167 int threshold;
9e917af1 2168
0a0aba42
MM		 2169 BT_DBG("chan %p last_acked_seq %d buffer_seq %d",
2170 chan, chan->last_acked_seq, chan->buffer_seq);
9e917af1 2171
0a0aba42
MM		 2172 memset(&control, 0, sizeof(control));
2173 control.sframe = 1;
dfc909be 2174
0a0aba42
MM		 2175 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state) &&
2176 chan->rx_state == L2CAP_RX_STATE_RECV) {
2177 __clear_ack_timer(chan);
2178 control.super = L2CAP_SUPER_RNR;
2179 control.reqseq = chan->buffer_seq;
2180 l2cap_send_sframe(chan, &control);
2181 } else {
2182 if (!test_bit(CONN_REMOTE_BUSY, &chan->conn_state)) {
2183 l2cap_ertm_send(chan);
2184 /* If any i-frames were sent, they included an ack */
2185 if (chan->buffer_seq == chan->last_acked_seq)
2186 frames_to_ack = 0;
2187 }
dfc909be 2188
c20f8e35 2189 /* Ack now if the window is 3/4ths full.
0a0aba42
MM		 2190 * Calculate without mul or div
2191 */
c20f8e35 2192 threshold = chan->ack_win;
0a0aba42
MM		 2193 threshold += threshold << 1;
2194 threshold >>= 2;
2195
b4400672 2196 BT_DBG("frames_to_ack %u, threshold %d", frames_to_ack,
0a0aba42
MM		 2197 threshold);
2198
2199 if (frames_to_ack >= threshold) {
2200 __clear_ack_timer(chan);
2201 control.super = L2CAP_SUPER_RR;
2202 control.reqseq = chan->buffer_seq;
2203 l2cap_send_sframe(chan, &control);
2204 frames_to_ack = 0;
2205 }
1c2acffb 2206
0a0aba42
MM		 2207 if (frames_to_ack)
2208 __set_ack_timer(chan);
2209 }
b17e73bb
SJ		 2210}
2211
04124681
GP		 2212static inline int l2cap_skbuff_fromiovec(struct l2cap_chan *chan,
2213 struct msghdr *msg, int len,
2214 int count, struct sk_buff *skb)
1c2acffb 2215{
0952a57a 2216 struct l2cap_conn *conn = chan->conn;
1c2acffb 2217 struct sk_buff **frag;
90338947 2218 int sent = 0;
1da177e4 2219
59203a21 2220 if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count))
1c2acffb 2221 return -EFAULT;
1da177e4
LT		 2222
2223 sent += count;
2224 len -= count;
2225
2226 /* Continuation fragments (no L2CAP header) */
2227 frag = &skb_shinfo(skb)->frag_list;
2228 while (len) {
fbe00700
GP		 2229 struct sk_buff *tmp;
2230
1da177e4
LT		 2231 count = min_t(unsigned int, conn->mtu, len);
2232
fbe00700
GP		 2233 tmp = chan->ops->alloc_skb(chan, count,
2234 msg->msg_flags & MSG_DONTWAIT);
2235 if (IS_ERR(tmp))
2236 return PTR_ERR(tmp);
2237
2238 *frag = tmp;
2f7719ce 2239
1c2acffb
GP		 2240 if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count))
2241 return -EFAULT;
1da177e4 2242
5e59b791
LAD		 2243 (*frag)->priority = skb->priority;
2244
1da177e4
LT		 2245 sent += count;
2246 len -= count;
2247
2d0ed3d5
GP		 2248 skb->len += (*frag)->len;
2249 skb->data_len += (*frag)->len;
2250
1da177e4
LT		 2251 frag = &(*frag)->next;
2252 }
1da177e4
LT		 2253
2254 return sent;
1c2acffb 2255}
1da177e4 2256
5e59b791 2257static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan,
b4400672
AE		 2258 struct msghdr *msg, size_t len,
2259 u32 priority)
1c2acffb 2260{
8c1d787b 2261 struct l2cap_conn *conn = chan->conn;
1c2acffb 2262 struct sk_buff *skb;
03a51213 2263 int err, count, hlen = L2CAP_HDR_SIZE + L2CAP_PSMLEN_SIZE;
1c2acffb
GP		 2264 struct l2cap_hdr *lh;
2265
b4400672 2266 BT_DBG("chan %p len %zu priority %u", chan, len, priority);
1c2acffb
GP		 2267
2268 count = min_t(unsigned int, (conn->mtu - hlen), len);
2f7719ce
AE		 2269
2270 skb = chan->ops->alloc_skb(chan, count + hlen,
90338947
GP		 2271 msg->msg_flags & MSG_DONTWAIT);
2272 if (IS_ERR(skb))
2273 return skb;
1c2acffb 2274
5e59b791
LAD		 2275 skb->priority = priority;
2276
1c2acffb
GP		 2277 /* Create L2CAP header */
2278 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
fe4128e0 2279 lh->cid = cpu_to_le16(chan->dcid);
daf6a78c
AE		 2280 lh->len = cpu_to_le16(len + L2CAP_PSMLEN_SIZE);
2281 put_unaligned(chan->psm, skb_put(skb, L2CAP_PSMLEN_SIZE));
1c2acffb 2282
0952a57a 2283 err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb);
1c2acffb
GP		 2284 if (unlikely(err < 0)) {
2285 kfree_skb(skb);
2286 return ERR_PTR(err);
2287 }
2288 return skb;
2289}
2290
5e59b791 2291static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan,
b4400672
AE		 2292 struct msghdr *msg, size_t len,
2293 u32 priority)
1c2acffb 2294{
8c1d787b 2295 struct l2cap_conn *conn = chan->conn;
1c2acffb 2296 struct sk_buff *skb;
f2ba7fae 2297 int err, count;
1c2acffb
GP		 2298 struct l2cap_hdr *lh;
2299
b4400672 2300 BT_DBG("chan %p len %zu", chan, len);
1c2acffb 2301
f2ba7fae 2302 count = min_t(unsigned int, (conn->mtu - L2CAP_HDR_SIZE), len);
2f7719ce 2303
f2ba7fae 2304 skb = chan->ops->alloc_skb(chan, count + L2CAP_HDR_SIZE,
90338947
GP		 2305 msg->msg_flags & MSG_DONTWAIT);
2306 if (IS_ERR(skb))
2307 return skb;
1c2acffb 2308
5e59b791
LAD		 2309 skb->priority = priority;
2310
1c2acffb
GP		 2311 /* Create L2CAP header */
2312 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
fe4128e0 2313 lh->cid = cpu_to_le16(chan->dcid);
6ff9b5ef 2314 lh->len = cpu_to_le16(len);
1c2acffb 2315
0952a57a 2316 err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb);
1c2acffb
GP		 2317 if (unlikely(err < 0)) {
2318 kfree_skb(skb);
2319 return ERR_PTR(err);
2320 }
2321 return skb;
2322}
2323
ab0ff76d 2324static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan,
b4400672
AE		 2325 struct msghdr *msg, size_t len,
2326 u16 sdulen)
1c2acffb 2327{
8c1d787b 2328 struct l2cap_conn *conn = chan->conn;
1c2acffb 2329 struct sk_buff *skb;
e4ca6d98 2330 int err, count, hlen;
1c2acffb
GP		 2331 struct l2cap_hdr *lh;
2332
b4400672 2333 BT_DBG("chan %p len %zu", chan, len);
1c2acffb 2334
0ee0d208
GP		 2335 if (!conn)
2336 return ERR_PTR(-ENOTCONN);
2337
ba7aa64f 2338 hlen = __ertm_hdr_size(chan);
e4ca6d98 2339
c74e560c 2340 if (sdulen)
03a51213 2341 hlen += L2CAP_SDULEN_SIZE;
c74e560c 2342
47d1ec61 2343 if (chan->fcs == L2CAP_FCS_CRC16)
03a51213 2344 hlen += L2CAP_FCS_SIZE;
fcc203c3 2345
1c2acffb 2346 count = min_t(unsigned int, (conn->mtu - hlen), len);
2f7719ce
AE		 2347
2348 skb = chan->ops->alloc_skb(chan, count + hlen,
90338947
GP		 2349 msg->msg_flags & MSG_DONTWAIT);
2350 if (IS_ERR(skb))
2351 return skb;
1c2acffb
GP		 2352
2353 /* Create L2CAP header */
2354 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
fe4128e0 2355 lh->cid = cpu_to_le16(chan->dcid);
1c2acffb 2356 lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
88843ab0 2357
18a48e76
MM		 2358 /* Control header is populated later */
2359 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
2360 put_unaligned_le32(0, skb_put(skb, L2CAP_EXT_CTRL_SIZE));
2361 else
2362 put_unaligned_le16(0, skb_put(skb, L2CAP_ENH_CTRL_SIZE));
88843ab0 2363
c74e560c 2364 if (sdulen)
03a51213 2365 put_unaligned_le16(sdulen, skb_put(skb, L2CAP_SDULEN_SIZE));
1c2acffb 2366
0952a57a 2367 err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb);
1c2acffb
GP		 2368 if (unlikely(err < 0)) {
2369 kfree_skb(skb);
2370 return ERR_PTR(err);
2371 }
e90bac06 2372
18a48e76 2373 bt_cb(skb)->control.fcs = chan->fcs;
3ce3514f 2374 bt_cb(skb)->control.retries = 0;
1c2acffb 2375 return skb;
1da177e4
LT		 2376}
2377
94122bbe
MM		 2378static int l2cap_segment_sdu(struct l2cap_chan *chan,
2379 struct sk_buff_head *seg_queue,
2380 struct msghdr *msg, size_t len)
c74e560c 2381{
c74e560c 2382 struct sk_buff *skb;
94122bbe
MM		 2383 u16 sdu_len;
2384 size_t pdu_len;
94122bbe 2385 u8 sar;
c74e560c 2386
b4400672 2387 BT_DBG("chan %p, msg %p, len %zu", chan, msg, len);
c74e560c 2388
94122bbe
MM		 2389 /* It is critical that ERTM PDUs fit in a single HCI fragment,
2390 * so fragmented skbs are not used. The HCI layer's handling
2391 * of fragmented skbs is not compatible with ERTM's queueing.
2392 */
c74e560c 2393
94122bbe
MM		 2394 /* PDU size is derived from the HCI MTU */
2395 pdu_len = chan->conn->mtu;
c74e560c 2396
a549574d
MM		 2397 /* Constrain PDU size for BR/EDR connections */
2398 if (!chan->hs_hcon)
2399 pdu_len = min_t(size_t, pdu_len, L2CAP_BREDR_MAX_PAYLOAD);
94122bbe
MM		 2400
2401 /* Adjust for largest possible L2CAP overhead. */
35d401df
GP		 2402 if (chan->fcs)
2403 pdu_len -= L2CAP_FCS_SIZE;
2404
ba7aa64f 2405 pdu_len -= __ertm_hdr_size(chan);
94122bbe
MM		 2406
2407 /* Remote device may have requested smaller PDUs */
2408 pdu_len = min_t(size_t, pdu_len, chan->remote_mps);
2409
2410 if (len <= pdu_len) {
2411 sar = L2CAP_SAR_UNSEGMENTED;
2412 sdu_len = 0;
2413 pdu_len = len;
2414 } else {
2415 sar = L2CAP_SAR_START;
2416 sdu_len = len;
2417 pdu_len -= L2CAP_SDULEN_SIZE;
2418 }
2419
2420 while (len > 0) {
2421 skb = l2cap_create_iframe_pdu(chan, msg, pdu_len, sdu_len);
c74e560c 2422
c74e560c 2423 if (IS_ERR(skb)) {
94122bbe 2424 __skb_queue_purge(seg_queue);
c74e560c
GP		 2425 return PTR_ERR(skb);
2426 }
2427
94122bbe
MM		 2428 bt_cb(skb)->control.sar = sar;
2429 __skb_queue_tail(seg_queue, skb);
2430
2431 len -= pdu_len;
2432 if (sdu_len) {
2433 sdu_len = 0;
2434 pdu_len += L2CAP_SDULEN_SIZE;
2435 }
2436
2437 if (len <= pdu_len) {
2438 sar = L2CAP_SAR_END;
2439 pdu_len = len;
2440 } else {
2441 sar = L2CAP_SAR_CONTINUE;
2442 }
c74e560c 2443 }
c74e560c 2444
f0f62799 2445 return 0;
c74e560c
GP		 2446}
2447
5e59b791 2448int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len,
2d792818 2449 u32 priority)
9a91a04a 2450{
9a91a04a 2451 struct sk_buff *skb;
9a91a04a 2452 int err;
94122bbe 2453 struct sk_buff_head seg_queue;
9a91a04a
GP		 2454
2455 /* Connectionless channel */
715ec005 2456 if (chan->chan_type == L2CAP_CHAN_CONN_LESS) {
5e59b791 2457 skb = l2cap_create_connless_pdu(chan, msg, len, priority);
9a91a04a
GP		 2458 if (IS_ERR(skb))
2459 return PTR_ERR(skb);
2460
2461 l2cap_do_send(chan, skb);
2462 return len;
2463 }
2464
2465 switch (chan->mode) {
2466 case L2CAP_MODE_BASIC:
2467 /* Check outgoing MTU */
2468 if (len > chan->omtu)
2469 return -EMSGSIZE;
2470
2471 /* Create a basic PDU */
5e59b791 2472 skb = l2cap_create_basic_pdu(chan, msg, len, priority);
9a91a04a
GP		 2473 if (IS_ERR(skb))
2474 return PTR_ERR(skb);
2475
2476 l2cap_do_send(chan, skb);
2477 err = len;
2478 break;
2479
2480 case L2CAP_MODE_ERTM:
2481 case L2CAP_MODE_STREAMING:
94122bbe
MM		 2482 /* Check outgoing MTU */
2483 if (len > chan->omtu) {
2484 err = -EMSGSIZE;
2485 break;
2486 }
9a91a04a 2487
94122bbe 2488 __skb_queue_head_init(&seg_queue);
9a91a04a 2489
94122bbe
MM		 2490 /* Do segmentation before calling in to the state machine,
2491 * since it's possible to block while waiting for memory
2492 * allocation.
2493 */
2494 err = l2cap_segment_sdu(chan, &seg_queue, msg, len);
9a91a04a 2495
94122bbe
MM		 2496 /* The channel could have been closed while segmenting,
2497 * check that it is still connected.
2498 */
2499 if (chan->state != BT_CONNECTED) {
2500 __skb_queue_purge(&seg_queue);
2501 err = -ENOTCONN;
9a91a04a
GP		 2502 }
2503
94122bbe 2504 if (err)
9a91a04a 2505 break;
9a91a04a 2506
3733937d 2507 if (chan->mode == L2CAP_MODE_ERTM)
d660366d 2508 l2cap_tx(chan, NULL, &seg_queue, L2CAP_EV_DATA_REQUEST);
3733937d 2509 else
d660366d 2510 l2cap_streaming_send(chan, &seg_queue);
9a91a04a 2511
d660366d 2512 err = len;
9a91a04a 2513
94122bbe
MM		 2514 /* If the skbs were not queued for sending, they'll still be in
2515 * seg_queue and need to be purged.
2516 */
2517 __skb_queue_purge(&seg_queue);
9a91a04a
GP		 2518 break;
2519
2520 default:
2521 BT_DBG("bad state %1.1x", chan->mode);
2522 err = -EBADFD;
2523 }
2524
2525 return err;
2526}
2527
d2a7ac5d
MM		 2528static void l2cap_send_srej(struct l2cap_chan *chan, u16 txseq)
2529{
bed68bde
MM		 2530 struct l2cap_ctrl control;
2531 u16 seq;
2532
b4400672 2533 BT_DBG("chan %p, txseq %u", chan, txseq);
bed68bde
MM		 2534
2535 memset(&control, 0, sizeof(control));
2536 control.sframe = 1;
2537 control.super = L2CAP_SUPER_SREJ;
2538
2539 for (seq = chan->expected_tx_seq; seq != txseq;
2540 seq = __next_seq(chan, seq)) {
2541 if (!l2cap_ertm_seq_in_queue(&chan->srej_q, seq)) {
2542 control.reqseq = seq;
2543 l2cap_send_sframe(chan, &control);
2544 l2cap_seq_list_append(&chan->srej_list, seq);
2545 }
2546 }
2547
2548 chan->expected_tx_seq = __next_seq(chan, txseq);
d2a7ac5d
MM		 2549}
2550
2551static void l2cap_send_srej_tail(struct l2cap_chan *chan)
2552{
bed68bde
MM		 2553 struct l2cap_ctrl control;
2554
2555 BT_DBG("chan %p", chan);
2556
2557 if (chan->srej_list.tail == L2CAP_SEQ_LIST_CLEAR)
2558 return;
2559
2560 memset(&control, 0, sizeof(control));
2561 control.sframe = 1;
2562 control.super = L2CAP_SUPER_SREJ;
2563 control.reqseq = chan->srej_list.tail;
2564 l2cap_send_sframe(chan, &control);
d2a7ac5d
MM		 2565}
2566
2567static void l2cap_send_srej_list(struct l2cap_chan *chan, u16 txseq)
2568{
bed68bde
MM		 2569 struct l2cap_ctrl control;
2570 u16 initial_head;
2571 u16 seq;
2572
b4400672 2573 BT_DBG("chan %p, txseq %u", chan, txseq);
bed68bde
MM		 2574
2575 memset(&control, 0, sizeof(control));
2576 control.sframe = 1;
2577 control.super = L2CAP_SUPER_SREJ;
2578
2579 /* Capture initial list head to allow only one pass through the list. */
2580 initial_head = chan->srej_list.head;
2581
2582 do {
2583 seq = l2cap_seq_list_pop(&chan->srej_list);
2584 if (seq == txseq || seq == L2CAP_SEQ_LIST_CLEAR)
2585 break;
2586
2587 control.reqseq = seq;
2588 l2cap_send_sframe(chan, &control);
2589 l2cap_seq_list_append(&chan->srej_list, seq);
2590 } while (chan->srej_list.head != initial_head);
d2a7ac5d
MM		 2591}
2592
608bcc6d
MM		 2593static void l2cap_process_reqseq(struct l2cap_chan *chan, u16 reqseq)
2594{
2595 struct sk_buff *acked_skb;
2596 u16 ackseq;
2597
b4400672 2598 BT_DBG("chan %p, reqseq %u", chan, reqseq);
608bcc6d
MM		 2599
2600 if (chan->unacked_frames == 0 || reqseq == chan->expected_ack_seq)
2601 return;
2602
b4400672 2603 BT_DBG("expected_ack_seq %u, unacked_frames %u",
608bcc6d
MM		 2604 chan->expected_ack_seq, chan->unacked_frames);
2605
2606 for (ackseq = chan->expected_ack_seq; ackseq != reqseq;
2607 ackseq = __next_seq(chan, ackseq)) {
2608
2609 acked_skb = l2cap_ertm_seq_in_queue(&chan->tx_q, ackseq);
2610 if (acked_skb) {
2611 skb_unlink(acked_skb, &chan->tx_q);
2612 kfree_skb(acked_skb);
2613 chan->unacked_frames--;
2614 }
2615 }
2616
2617 chan->expected_ack_seq = reqseq;
2618
2619 if (chan->unacked_frames == 0)
2620 __clear_retrans_timer(chan);
2621
b4400672 2622 BT_DBG("unacked_frames %u", chan->unacked_frames);
608bcc6d
MM		 2623}
2624
2625static void l2cap_abort_rx_srej_sent(struct l2cap_chan *chan)
2626{
2627 BT_DBG("chan %p", chan);
2628
2629 chan->expected_tx_seq = chan->buffer_seq;
2630 l2cap_seq_list_clear(&chan->srej_list);
2631 skb_queue_purge(&chan->srej_q);
2632 chan->rx_state = L2CAP_RX_STATE_RECV;
2633}
2634
d660366d
GP		 2635static void l2cap_tx_state_xmit(struct l2cap_chan *chan,
2636 struct l2cap_ctrl *control,
2637 struct sk_buff_head *skbs, u8 event)
608bcc6d 2638{
608bcc6d
MM		 2639 BT_DBG("chan %p, control %p, skbs %p, event %d", chan, control, skbs,
2640 event);
2641
2642 switch (event) {
2643 case L2CAP_EV_DATA_REQUEST:
2644 if (chan->tx_send_head == NULL)
2645 chan->tx_send_head = skb_peek(skbs);
2646
2647 skb_queue_splice_tail_init(skbs, &chan->tx_q);
2648 l2cap_ertm_send(chan);
2649 break;
2650 case L2CAP_EV_LOCAL_BUSY_DETECTED:
2651 BT_DBG("Enter LOCAL_BUSY");
2652 set_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2653
2654 if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
2655 /* The SREJ_SENT state must be aborted if we are to
2656 * enter the LOCAL_BUSY state.
2657 */
2658 l2cap_abort_rx_srej_sent(chan);
2659 }
2660
2661 l2cap_send_ack(chan);
2662
2663 break;
2664 case L2CAP_EV_LOCAL_BUSY_CLEAR:
2665 BT_DBG("Exit LOCAL_BUSY");
2666 clear_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2667
2668 if (test_bit(CONN_RNR_SENT, &chan->conn_state)) {
2669 struct l2cap_ctrl local_control;
2670
2671 memset(&local_control, 0, sizeof(local_control));
2672 local_control.sframe = 1;
2673 local_control.super = L2CAP_SUPER_RR;
2674 local_control.poll = 1;
2675 local_control.reqseq = chan->buffer_seq;
a67d7f6f 2676 l2cap_send_sframe(chan, &local_control);
608bcc6d
MM		 2677
2678 chan->retry_count = 1;
2679 __set_monitor_timer(chan);
2680 chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2681 }
2682 break;
2683 case L2CAP_EV_RECV_REQSEQ_AND_FBIT:
2684 l2cap_process_reqseq(chan, control->reqseq);
2685 break;
2686 case L2CAP_EV_EXPLICIT_POLL:
2687 l2cap_send_rr_or_rnr(chan, 1);
2688 chan->retry_count = 1;
2689 __set_monitor_timer(chan);
2690 __clear_ack_timer(chan);
2691 chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2692 break;
2693 case L2CAP_EV_RETRANS_TO:
2694 l2cap_send_rr_or_rnr(chan, 1);
2695 chan->retry_count = 1;
2696 __set_monitor_timer(chan);
2697 chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2698 break;
2699 case L2CAP_EV_RECV_FBIT:
2700 /* Nothing to process */
2701 break;
2702 default:
2703 break;
2704 }
608bcc6d
MM		 2705}
2706
d660366d
GP		 2707static void l2cap_tx_state_wait_f(struct l2cap_chan *chan,
2708 struct l2cap_ctrl *control,
2709 struct sk_buff_head *skbs, u8 event)
608bcc6d 2710{
608bcc6d
MM		 2711 BT_DBG("chan %p, control %p, skbs %p, event %d", chan, control, skbs,
2712 event);
2713
2714 switch (event) {
2715 case L2CAP_EV_DATA_REQUEST:
2716 if (chan->tx_send_head == NULL)
2717 chan->tx_send_head = skb_peek(skbs);
2718 /* Queue data, but don't send. */
2719 skb_queue_splice_tail_init(skbs, &chan->tx_q);
2720 break;
2721 case L2CAP_EV_LOCAL_BUSY_DETECTED:
2722 BT_DBG("Enter LOCAL_BUSY");
2723 set_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2724
2725 if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
2726 /* The SREJ_SENT state must be aborted if we are to
2727 * enter the LOCAL_BUSY state.
2728 */
2729 l2cap_abort_rx_srej_sent(chan);
2730 }
2731
2732 l2cap_send_ack(chan);
2733
2734 break;
2735 case L2CAP_EV_LOCAL_BUSY_CLEAR:
2736 BT_DBG("Exit LOCAL_BUSY");
2737 clear_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2738
2739 if (test_bit(CONN_RNR_SENT, &chan->conn_state)) {
2740 struct l2cap_ctrl local_control;
2741 memset(&local_control, 0, sizeof(local_control));
2742 local_control.sframe = 1;
2743 local_control.super = L2CAP_SUPER_RR;
2744 local_control.poll = 1;
2745 local_control.reqseq = chan->buffer_seq;
a67d7f6f 2746 l2cap_send_sframe(chan, &local_control);
608bcc6d
MM		 2747
2748 chan->retry_count = 1;
2749 __set_monitor_timer(chan);
2750 chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2751 }
2752 break;
2753 case L2CAP_EV_RECV_REQSEQ_AND_FBIT:
2754 l2cap_process_reqseq(chan, control->reqseq);
2755
2756 /* Fall through */
2757
2758 case L2CAP_EV_RECV_FBIT:
2759 if (control && control->final) {
2760 __clear_monitor_timer(chan);
2761 if (chan->unacked_frames > 0)
2762 __set_retrans_timer(chan);
2763 chan->retry_count = 0;
2764 chan->tx_state = L2CAP_TX_STATE_XMIT;
2765 BT_DBG("recv fbit tx_state 0x2.2%x", chan->tx_state);
2766 }
2767 break;
2768 case L2CAP_EV_EXPLICIT_POLL:
2769 /* Ignore */
2770 break;
2771 case L2CAP_EV_MONITOR_TO:
2772 if (chan->max_tx == 0 || chan->retry_count < chan->max_tx) {
2773 l2cap_send_rr_or_rnr(chan, 1);
2774 __set_monitor_timer(chan);
2775 chan->retry_count++;
2776 } else {
5e4e3972 2777 l2cap_send_disconn_req(chan, ECONNABORTED);
608bcc6d
MM		 2778 }
2779 break;
2780 default:
2781 break;
2782 }
608bcc6d
MM		 2783}
2784
d660366d
GP		 2785static void l2cap_tx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
2786 struct sk_buff_head *skbs, u8 event)
608bcc6d 2787{
608bcc6d
MM		 2788 BT_DBG("chan %p, control %p, skbs %p, event %d, state %d",
2789 chan, control, skbs, event, chan->tx_state);
2790
2791 switch (chan->tx_state) {
2792 case L2CAP_TX_STATE_XMIT:
d660366d 2793 l2cap_tx_state_xmit(chan, control, skbs, event);
608bcc6d
MM		 2794 break;
2795 case L2CAP_TX_STATE_WAIT_F:
d660366d 2796 l2cap_tx_state_wait_f(chan, control, skbs, event);
608bcc6d
MM		 2797 break;
2798 default:
2799 /* Ignore event */
2800 break;
2801 }
608bcc6d
MM		 2802}
2803
4b51dae9
MM		 2804static void l2cap_pass_to_tx(struct l2cap_chan *chan,
2805 struct l2cap_ctrl *control)
2806{
2807 BT_DBG("chan %p, control %p", chan, control);
401bb1f7 2808 l2cap_tx(chan, control, NULL, L2CAP_EV_RECV_REQSEQ_AND_FBIT);
4b51dae9
MM		 2809}
2810
f80842a8
MM		 2811static void l2cap_pass_to_tx_fbit(struct l2cap_chan *chan,
2812 struct l2cap_ctrl *control)
2813{
2814 BT_DBG("chan %p, control %p", chan, control);
401bb1f7 2815 l2cap_tx(chan, control, NULL, L2CAP_EV_RECV_FBIT);
f80842a8
MM		 2816}
2817
1da177e4
LT		 2818/* Copy frame to all raw sockets on that connection */
2819static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb)
2820{
1da177e4 2821 struct sk_buff *nskb;
48454079 2822 struct l2cap_chan *chan;
1da177e4
LT		 2823
2824 BT_DBG("conn %p", conn);
2825
3df91ea2 2826 mutex_lock(&conn->chan_lock);
3d57dc68 2827
3df91ea2 2828 list_for_each_entry(chan, &conn->chan_l, list) {
48454079 2829 struct sock *sk = chan->sk;
715ec005 2830 if (chan->chan_type != L2CAP_CHAN_RAW)
1da177e4
LT		 2831 continue;
2832
2833 /* Don't send frame to the socket it came from */
2834 if (skb->sk == sk)
2835 continue;
8bcde1f2 2836 nskb = skb_clone(skb, GFP_KERNEL);
af05b30b 2837 if (!nskb)
1da177e4
LT		 2838 continue;
2839
80b98027 2840 if (chan->ops->recv(chan, nskb))
1da177e4
LT		 2841 kfree_skb(nskb);
2842 }
3d57dc68 2843
3df91ea2 2844 mutex_unlock(&conn->chan_lock);
1da177e4
LT		 2845}
2846
2847/* ---- L2CAP signalling commands ---- */
b4400672
AE		 2848static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn, u8 code,
2849 u8 ident, u16 dlen, void *data)
1da177e4
LT		 2850{
2851 struct sk_buff *skb, **frag;
2852 struct l2cap_cmd_hdr *cmd;
2853 struct l2cap_hdr *lh;
2854 int len, count;
2855
b4400672
AE		 2856 BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %u",
2857 conn, code, ident, dlen);
1da177e4 2858
300b962e
AL		 2859 if (conn->mtu < L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE)
2860 return NULL;
2861
1da177e4
LT		 2862 len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen;
2863 count = min_t(unsigned int, conn->mtu, len);
2864
8bcde1f2 2865 skb = bt_skb_alloc(count, GFP_KERNEL);
1da177e4
LT		 2866 if (!skb)
2867 return NULL;
2868
2869 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
aca3192c 2870 lh->len = cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen);
3300d9a9
CT		 2871
2872 if (conn->hcon->type == LE_LINK)
ac73498c 2873 lh->cid = __constant_cpu_to_le16(L2CAP_CID_LE_SIGNALING);
3300d9a9 2874 else
ac73498c 2875 lh->cid = __constant_cpu_to_le16(L2CAP_CID_SIGNALING);
1da177e4
LT		 2876
2877 cmd = (struct l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE);
2878 cmd->code = code;
2879 cmd->ident = ident;
aca3192c 2880 cmd->len = cpu_to_le16(dlen);
1da177e4
LT		 2881
2882 if (dlen) {
2883 count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE;
2884 memcpy(skb_put(skb, count), data, count);
2885 data += count;
2886 }
2887
2888 len -= skb->len;
2889
2890 /* Continuation fragments (no L2CAP header) */
2891 frag = &skb_shinfo(skb)->frag_list;
2892 while (len) {
2893 count = min_t(unsigned int, conn->mtu, len);
2894
8bcde1f2 2895 *frag = bt_skb_alloc(count, GFP_KERNEL);
1da177e4
LT		 2896 if (!*frag)
2897 goto fail;
2898
2899 memcpy(skb_put(*frag, count), data, count);
2900
2901 len -= count;
2902 data += count;
2903
2904 frag = &(*frag)->next;
2905 }
2906
2907 return skb;
2908
2909fail:
2910 kfree_skb(skb);
2911 return NULL;
2912}
2913
2d792818
GP		 2914static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen,
2915 unsigned long *val)
1da177e4
LT		 2916{
2917 struct l2cap_conf_opt *opt = *ptr;
2918 int len;
2919
2920 len = L2CAP_CONF_OPT_SIZE + opt->len;
2921 *ptr += len;
2922
2923 *type = opt->type;
2924 *olen = opt->len;
2925
2926 switch (opt->len) {
2927 case 1:
2928 *val = *((u8 *) opt->val);
2929 break;
2930
2931 case 2:
bfaaeb3e 2932 *val = get_unaligned_le16(opt->val);
1da177e4
LT		 2933 break;
2934
2935 case 4:
bfaaeb3e 2936 *val = get_unaligned_le32(opt->val);
1da177e4
LT		 2937 break;
2938
2939 default:
2940 *val = (unsigned long) opt->val;
2941 break;
2942 }
2943
b4400672 2944 BT_DBG("type 0x%2.2x len %u val 0x%lx", *type, opt->len, *val);
1da177e4
LT		 2945 return len;
2946}
2947
1da177e4
LT		 2948static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val)
2949{
2950 struct l2cap_conf_opt *opt = *ptr;
2951
b4400672 2952 BT_DBG("type 0x%2.2x len %u val 0x%lx", type, len, val);
1da177e4
LT		 2953
2954 opt->type = type;
2955 opt->len = len;
2956
2957 switch (len) {
2958 case 1:
2959 *((u8 *) opt->val) = val;
2960 break;
2961
2962 case 2:
4f8b691c 2963 put_unaligned_le16(val, opt->val);
1da177e4
LT		 2964 break;
2965
2966 case 4:
4f8b691c 2967 put_unaligned_le32(val, opt->val);
1da177e4
LT		 2968 break;
2969
2970 default:
2971 memcpy(opt->val, (void *) val, len);
2972 break;
2973 }
2974
2975 *ptr += L2CAP_CONF_OPT_SIZE + len;
2976}
2977
f89cef09
AE		 2978static void l2cap_add_opt_efs(void **ptr, struct l2cap_chan *chan)
2979{
2980 struct l2cap_conf_efs efs;
2981
1ec918ce 2982 switch (chan->mode) {
f89cef09
AE		 2983 case L2CAP_MODE_ERTM:
2984 efs.id = chan->local_id;
2985 efs.stype = chan->local_stype;
2986 efs.msdu = cpu_to_le16(chan->local_msdu);
2987 efs.sdu_itime = cpu_to_le32(chan->local_sdu_itime);
ac73498c 2988 efs.acc_lat = __constant_cpu_to_le32(L2CAP_DEFAULT_ACC_LAT);
8936fa6d 2989 efs.flush_to = __constant_cpu_to_le32(L2CAP_EFS_DEFAULT_FLUSH_TO);
f89cef09
AE		 2990 break;
2991
2992 case L2CAP_MODE_STREAMING:
2993 efs.id = 1;
2994 efs.stype = L2CAP_SERV_BESTEFFORT;
2995 efs.msdu = cpu_to_le16(chan->local_msdu);
2996 efs.sdu_itime = cpu_to_le32(chan->local_sdu_itime);
2997 efs.acc_lat = 0;
2998 efs.flush_to = 0;
2999 break;
3000
3001 default:
3002 return;
3003 }
3004
3005 l2cap_add_conf_opt(ptr, L2CAP_CONF_EFS, sizeof(efs),
8936fa6d 3006 (unsigned long) &efs);
f89cef09
AE		 3007}
3008
721c4181 3009static void l2cap_ack_timeout(struct work_struct *work)
c1b4f43b 3010{
721c4181 3011 struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
0362520b
MM		 3012 ack_timer.work);
3013 u16 frames_to_ack;
c1b4f43b 3014
2fb9b3d4
GP		 3015 BT_DBG("chan %p", chan);
3016
6be36555
AE		 3017 l2cap_chan_lock(chan);
3018
0362520b
MM		 3019 frames_to_ack = __seq_offset(chan, chan->buffer_seq,
3020 chan->last_acked_seq);
6be36555 3021
0362520b
MM		 3022 if (frames_to_ack)
3023 l2cap_send_rr_or_rnr(chan, 0);
09bfb2ee 3024
0362520b 3025 l2cap_chan_unlock(chan);
09bfb2ee 3026 l2cap_chan_put(chan);
c1b4f43b
GP		 3027}
3028
466f8004 3029int l2cap_ertm_init(struct l2cap_chan *chan)
0565c1c2 3030{
3c588192
MM		 3031 int err;
3032
105bdf9e
MM		 3033 chan->next_tx_seq = 0;
3034 chan->expected_tx_seq = 0;
42e5c802 3035 chan->expected_ack_seq = 0;
6a026610 3036 chan->unacked_frames = 0;
42e5c802 3037 chan->buffer_seq = 0;
6a026610 3038 chan->frames_sent = 0;
105bdf9e
MM		 3039 chan->last_acked_seq = 0;
3040 chan->sdu = NULL;
3041 chan->sdu_last_frag = NULL;
3042 chan->sdu_len = 0;
3043
d34c34fb
MM		 3044 skb_queue_head_init(&chan->tx_q);
3045
08333283
MM		 3046 chan->local_amp_id = 0;
3047 chan->move_id = 0;
3048 chan->move_state = L2CAP_MOVE_STABLE;
3049 chan->move_role = L2CAP_MOVE_ROLE_NONE;
3050
105bdf9e
MM		 3051 if (chan->mode != L2CAP_MODE_ERTM)
3052 return 0;
3053
3054 chan->rx_state = L2CAP_RX_STATE_RECV;
3055 chan->tx_state = L2CAP_TX_STATE_XMIT;
0565c1c2 3056
721c4181
GP		 3057 INIT_DELAYED_WORK(&chan->retrans_timer, l2cap_retrans_timeout);
3058 INIT_DELAYED_WORK(&chan->monitor_timer, l2cap_monitor_timeout);
3059 INIT_DELAYED_WORK(&chan->ack_timer, l2cap_ack_timeout);
0565c1c2 3060
f1c6775b 3061 skb_queue_head_init(&chan->srej_q);
1890d36b 3062
3c588192
MM		 3063 err = l2cap_seq_list_init(&chan->srej_list, chan->tx_win);
3064 if (err < 0)
3065 return err;
3066
9dc9affc
MM		 3067 err = l2cap_seq_list_init(&chan->retrans_list, chan->remote_tx_win);
3068 if (err < 0)
3069 l2cap_seq_list_free(&chan->srej_list);
3070
3071 return err;
0565c1c2
GP		 3072}
3073
f2fcfcd6
GP		 3074static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask)
3075{
3076 switch (mode) {
3077 case L2CAP_MODE_STREAMING:
3078 case L2CAP_MODE_ERTM:
3079 if (l2cap_mode_supported(mode, remote_feat_mask))
3080 return mode;
3081 /* fall through */
3082 default:
3083 return L2CAP_MODE_BASIC;
3084 }
3085}
3086
6327eb98
AE		 3087static inline bool __l2cap_ews_supported(struct l2cap_chan *chan)
3088{
3089 return enable_hs && chan->conn->feat_mask & L2CAP_FEAT_EXT_WINDOW;
3090}
3091
f89cef09
AE		 3092static inline bool __l2cap_efs_supported(struct l2cap_chan *chan)
3093{
3094 return enable_hs && chan->conn->feat_mask & L2CAP_FEAT_EXT_FLOW;
3095}
3096
36c86c85
MM		 3097static void __l2cap_set_ertm_timeouts(struct l2cap_chan *chan,
3098 struct l2cap_conf_rfc *rfc)
3099{
3100 if (chan->local_amp_id && chan->hs_hcon) {
3101 u64 ertm_to = chan->hs_hcon->hdev->amp_be_flush_to;
3102
3103 /* Class 1 devices have must have ERTM timeouts
3104 * exceeding the Link Supervision Timeout. The
3105 * default Link Supervision Timeout for AMP
3106 * controllers is 10 seconds.
3107 *
3108 * Class 1 devices use 0xffffffff for their
3109 * best-effort flush timeout, so the clamping logic
3110 * will result in a timeout that meets the above
3111 * requirement. ERTM timeouts are 16-bit values, so
3112 * the maximum timeout is 65.535 seconds.
3113 */
3114
3115 /* Convert timeout to milliseconds and round */
3116 ertm_to = DIV_ROUND_UP_ULL(ertm_to, 1000);
3117
3118 /* This is the recommended formula for class 2 devices
3119 * that start ERTM timers when packets are sent to the
3120 * controller.
3121 */
3122 ertm_to = 3 * ertm_to + 500;
3123
3124 if (ertm_to > 0xffff)
3125 ertm_to = 0xffff;
3126
3127 rfc->retrans_timeout = cpu_to_le16((u16) ertm_to);
3128 rfc->monitor_timeout = rfc->retrans_timeout;
3129 } else {
3130 rfc->retrans_timeout = __constant_cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO);
3131 rfc->monitor_timeout = __constant_cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO);
3132 }
3133}
3134
6327eb98
AE		 3135static inline void l2cap_txwin_setup(struct l2cap_chan *chan)
3136{
3137 if (chan->tx_win > L2CAP_DEFAULT_TX_WINDOW &&
2d792818 3138 __l2cap_ews_supported(chan)) {
6327eb98
AE		 3139 /* use extended control field */
3140 set_bit(FLAG_EXT_CTRL, &chan->flags);
836be934
AE		 3141 chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
3142 } else {
6327eb98 3143 chan->tx_win = min_t(u16, chan->tx_win,
2d792818 3144 L2CAP_DEFAULT_TX_WINDOW);
836be934
AE		 3145 chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW;
3146 }
c20f8e35 3147 chan->ack_win = chan->tx_win;
6327eb98
AE		 3148}
3149
710f9b0a 3150static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data)
1da177e4 3151{
1da177e4 3152 struct l2cap_conf_req *req = data;
0c1bc5c6 3153 struct l2cap_conf_rfc rfc = { .mode = chan->mode };
1da177e4 3154 void *ptr = req->data;
c8f79162 3155 u16 size;
1da177e4 3156
49208c9c 3157 BT_DBG("chan %p", chan);
1da177e4 3158
73ffa904 3159 if (chan->num_conf_req || chan->num_conf_rsp)
f2fcfcd6
GP		 3160 goto done;
3161
0c1bc5c6 3162 switch (chan->mode) {
f2fcfcd6
GP		 3163 case L2CAP_MODE_STREAMING:
3164 case L2CAP_MODE_ERTM:
c1360a1c 3165 if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state))
85eb53c6 3166 break;
85eb53c6 3167
f89cef09
AE		 3168 if (__l2cap_efs_supported(chan))
3169 set_bit(FLAG_EFS_ENABLE, &chan->flags);
3170
2ba13ed6 3171 /* fall through */
f2fcfcd6 3172 default:
8c1d787b 3173 chan->mode = l2cap_select_mode(rfc.mode, chan->conn->feat_mask);
f2fcfcd6
GP		 3174 break;
3175 }
3176
3177done:
0c1bc5c6
GP		 3178 if (chan->imtu != L2CAP_DEFAULT_MTU)
3179 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu);
7990681c 3180
0c1bc5c6 3181 switch (chan->mode) {
65c7c491 3182 case L2CAP_MODE_BASIC:
8c1d787b 3183 if (!(chan->conn->feat_mask & L2CAP_FEAT_ERTM) &&
2d792818 3184 !(chan->conn->feat_mask & L2CAP_FEAT_STREAMING))
63406504
GP		 3185 break;
3186
62547752
GP		 3187 rfc.mode = L2CAP_MODE_BASIC;
3188 rfc.txwin_size = 0;
3189 rfc.max_transmit = 0;
3190 rfc.retrans_timeout = 0;
3191 rfc.monitor_timeout = 0;
3192 rfc.max_pdu_size = 0;
3193
63406504 3194 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
2d792818 3195 (unsigned long) &rfc);
65c7c491
MH		 3196 break;
3197
3198 case L2CAP_MODE_ERTM:
3199 rfc.mode = L2CAP_MODE_ERTM;
47d1ec61 3200 rfc.max_transmit = chan->max_tx;
36c86c85
MM		 3201
3202 __l2cap_set_ertm_timeouts(chan, &rfc);
c8f79162
AE		 3203
3204 size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
2d792818
GP		 3205 L2CAP_EXT_HDR_SIZE - L2CAP_SDULEN_SIZE -
3206 L2CAP_FCS_SIZE);
c8f79162 3207 rfc.max_pdu_size = cpu_to_le16(size);
f2fcfcd6 3208
6327eb98
AE		 3209 l2cap_txwin_setup(chan);
3210
3211 rfc.txwin_size = min_t(u16, chan->tx_win,
2d792818 3212 L2CAP_DEFAULT_TX_WINDOW);
f2fcfcd6 3213
63406504 3214 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
2d792818 3215 (unsigned long) &rfc);
63406504 3216
f89cef09
AE		 3217 if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
3218 l2cap_add_opt_efs(&ptr, chan);
3219
6327eb98
AE		 3220 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
3221 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
2d792818 3222 chan->tx_win);
60918918
AE		 3223
3224 if (chan->conn->feat_mask & L2CAP_FEAT_FCS)
3225 if (chan->fcs == L2CAP_FCS_NONE ||
f2592d3e 3226 test_bit(CONF_RECV_NO_FCS, &chan->conf_state)) {
60918918
AE		 3227 chan->fcs = L2CAP_FCS_NONE;
3228 l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1,
3229 chan->fcs);
3230 }
f2fcfcd6
GP		 3231 break;
3232
3233 case L2CAP_MODE_STREAMING:
273759e2 3234 l2cap_txwin_setup(chan);
f2fcfcd6
GP		 3235 rfc.mode = L2CAP_MODE_STREAMING;
3236 rfc.txwin_size = 0;
3237 rfc.max_transmit = 0;
3238 rfc.retrans_timeout = 0;
3239 rfc.monitor_timeout = 0;
c8f79162
AE		 3240
3241 size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
2d792818
GP		 3242 L2CAP_EXT_HDR_SIZE - L2CAP_SDULEN_SIZE -
3243 L2CAP_FCS_SIZE);
c8f79162 3244 rfc.max_pdu_size = cpu_to_le16(size);
65c7c491 3245
63406504 3246 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
2d792818 3247 (unsigned long) &rfc);
63406504 3248
f89cef09
AE		 3249 if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
3250 l2cap_add_opt_efs(&ptr, chan);
3251
60918918
AE		 3252 if (chan->conn->feat_mask & L2CAP_FEAT_FCS)
3253 if (chan->fcs == L2CAP_FCS_NONE ||
f2592d3e 3254 test_bit(CONF_RECV_NO_FCS, &chan->conf_state)) {
60918918
AE		 3255 chan->fcs = L2CAP_FCS_NONE;
3256 l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1,
3257 chan->fcs);
3258 }
65c7c491
MH		 3259 break;
3260 }
1da177e4 3261
fe4128e0 3262 req->dcid = cpu_to_le16(chan->dcid);
59e54bd1 3263 req->flags = __constant_cpu_to_le16(0);
1da177e4
LT		 3264
3265 return ptr - data;
3266}
3267
73ffa904 3268static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data)
1da177e4 3269{
5dee9e7c
MH		 3270 struct l2cap_conf_rsp *rsp = data;
3271 void *ptr = rsp->data;
73ffa904
GP		 3272 void *req = chan->conf_req;
3273 int len = chan->conf_len;
5dee9e7c
MH		 3274 int type, hint, olen;
3275 unsigned long val;
6464f35f 3276 struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
42dceae2
AE		 3277 struct l2cap_conf_efs efs;
3278 u8 remote_efs = 0;
861d6882 3279 u16 mtu = L2CAP_DEFAULT_MTU;
5dee9e7c 3280 u16 result = L2CAP_CONF_SUCCESS;
c8f79162 3281 u16 size;
1da177e4 3282
73ffa904 3283 BT_DBG("chan %p", chan);
820ae1b8 3284
5dee9e7c
MH		 3285 while (len >= L2CAP_CONF_OPT_SIZE) {
3286 len -= l2cap_get_conf_opt(&req, &type, &olen, &val);
1da177e4 3287
589d2746 3288 hint = type & L2CAP_CONF_HINT;
47ec1dcd 3289 type &= L2CAP_CONF_MASK;
5dee9e7c
MH		 3290
3291 switch (type) {
3292 case L2CAP_CONF_MTU:
861d6882 3293 mtu = val;
5dee9e7c
MH		 3294 break;
3295
3296 case L2CAP_CONF_FLUSH_TO:
0c1bc5c6 3297 chan->flush_to = val;
5dee9e7c
MH		 3298 break;
3299
3300 case L2CAP_CONF_QOS:
3301 break;
3302
6464f35f
MH		 3303 case L2CAP_CONF_RFC:
3304 if (olen == sizeof(rfc))
3305 memcpy(&rfc, (void *) val, olen);
3306 break;
3307
fcc203c3
GP		 3308 case L2CAP_CONF_FCS:
3309 if (val == L2CAP_FCS_NONE)
f2592d3e 3310 set_bit(CONF_RECV_NO_FCS, &chan->conf_state);
42dceae2 3311 break;
fcc203c3 3312
42dceae2
AE		 3313 case L2CAP_CONF_EFS:
3314 remote_efs = 1;
3315 if (olen == sizeof(efs))
3316 memcpy(&efs, (void *) val, olen);
fcc203c3
GP		 3317 break;
3318
6327eb98
AE		 3319 case L2CAP_CONF_EWS:
3320 if (!enable_hs)
3321 return -ECONNREFUSED;
fcc203c3 3322
6327eb98
AE		 3323 set_bit(FLAG_EXT_CTRL, &chan->flags);
3324 set_bit(CONF_EWS_RECV, &chan->conf_state);
836be934 3325 chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
6327eb98 3326 chan->remote_tx_win = val;
fcc203c3
GP		 3327 break;
3328
5dee9e7c
MH		 3329 default:
3330 if (hint)
3331 break;
3332
3333 result = L2CAP_CONF_UNKNOWN;
3334 *((u8 *) ptr++) = type;
3335 break;
3336 }
3337 }
3338
73ffa904 3339 if (chan->num_conf_rsp || chan->num_conf_req > 1)
f2fcfcd6
GP		 3340 goto done;
3341
0c1bc5c6 3342 switch (chan->mode) {
f2fcfcd6
GP		 3343 case L2CAP_MODE_STREAMING:
3344 case L2CAP_MODE_ERTM:
c1360a1c 3345 if (!test_bit(CONF_STATE2_DEVICE, &chan->conf_state)) {
0c1bc5c6 3346 chan->mode = l2cap_select_mode(rfc.mode,
2d792818 3347 chan->conn->feat_mask);
85eb53c6
GP		 3348 break;
3349 }
3350
42dceae2
AE		 3351 if (remote_efs) {
3352 if (__l2cap_efs_supported(chan))
3353 set_bit(FLAG_EFS_ENABLE, &chan->flags);
3354 else
3355 return -ECONNREFUSED;
3356 }
3357
0c1bc5c6 3358 if (chan->mode != rfc.mode)
f2fcfcd6 3359 return -ECONNREFUSED;
742e519b 3360
f2fcfcd6 3361 break;
f2fcfcd6
GP		 3362 }
3363
3364done:
0c1bc5c6 3365 if (chan->mode != rfc.mode) {
f2fcfcd6 3366 result = L2CAP_CONF_UNACCEPT;
0c1bc5c6 3367 rfc.mode = chan->mode;
f2fcfcd6 3368
73ffa904 3369 if (chan->num_conf_rsp == 1)
f2fcfcd6
GP		 3370 return -ECONNREFUSED;
3371
2d792818
GP		 3372 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3373 (unsigned long) &rfc);
f2fcfcd6
GP		 3374 }
3375
5dee9e7c
MH		 3376 if (result == L2CAP_CONF_SUCCESS) {
3377 /* Configure output options and let the other side know
3378 * which ones we don't like. */
3379
f2fcfcd6
GP		 3380 if (mtu < L2CAP_DEFAULT_MIN_MTU)
3381 result = L2CAP_CONF_UNACCEPT;
3382 else {
0c1bc5c6 3383 chan->omtu = mtu;
c1360a1c 3384 set_bit(CONF_MTU_DONE, &chan->conf_state);
f2fcfcd6 3385 }
0c1bc5c6 3386 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->omtu);
6464f35f 3387
42dceae2
AE		 3388 if (remote_efs) {
3389 if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
2d792818
GP		 3390 efs.stype != L2CAP_SERV_NOTRAFIC &&
3391 efs.stype != chan->local_stype) {
42dceae2
AE		 3392
3393 result = L2CAP_CONF_UNACCEPT;
3394
3395 if (chan->num_conf_req >= 1)
3396 return -ECONNREFUSED;
3397
3398 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
2d792818
GP		 3399 sizeof(efs),
3400 (unsigned long) &efs);
0e8b207e 3401 } else {
3e6b3b95 3402 /* Send PENDING Conf Rsp */
0e8b207e
AE		 3403 result = L2CAP_CONF_PENDING;
3404 set_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
42dceae2
AE		 3405 }
3406 }
3407
f2fcfcd6
GP		 3408 switch (rfc.mode) {
3409 case L2CAP_MODE_BASIC:
47d1ec61 3410 chan->fcs = L2CAP_FCS_NONE;
c1360a1c 3411 set_bit(CONF_MODE_DONE, &chan->conf_state);
f2fcfcd6
GP		 3412 break;
3413
3414 case L2CAP_MODE_ERTM:
6327eb98
AE		 3415 if (!test_bit(CONF_EWS_RECV, &chan->conf_state))
3416 chan->remote_tx_win = rfc.txwin_size;
3417 else
3418 rfc.txwin_size = L2CAP_DEFAULT_TX_WINDOW;
86b1b263 3419
2c03a7a4 3420 chan->remote_max_tx = rfc.max_transmit;
1c762159 3421
c8f79162 3422 size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
2d792818
GP		 3423 chan->conn->mtu - L2CAP_EXT_HDR_SIZE -
3424 L2CAP_SDULEN_SIZE - L2CAP_FCS_SIZE);
c8f79162
AE		 3425 rfc.max_pdu_size = cpu_to_le16(size);
3426 chan->remote_mps = size;
f2fcfcd6 3427
36c86c85 3428 __l2cap_set_ertm_timeouts(chan, &rfc);
f2fcfcd6 3429
c1360a1c 3430 set_bit(CONF_MODE_DONE, &chan->conf_state);
68ae6639
GP		 3431
3432 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2d792818 3433 sizeof(rfc), (unsigned long) &rfc);
68ae6639 3434
42dceae2
AE		 3435 if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
3436 chan->remote_id = efs.id;
3437 chan->remote_stype = efs.stype;
3438 chan->remote_msdu = le16_to_cpu(efs.msdu);
3439 chan->remote_flush_to =
2d792818 3440 le32_to_cpu(efs.flush_to);
42dceae2 3441 chan->remote_acc_lat =
2d792818 3442 le32_to_cpu(efs.acc_lat);
42dceae2
AE		 3443 chan->remote_sdu_itime =
3444 le32_to_cpu(efs.sdu_itime);
3445 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
2d792818
GP		 3446 sizeof(efs),
3447 (unsigned long) &efs);
42dceae2 3448 }
f2fcfcd6
GP		 3449 break;
3450
3451 case L2CAP_MODE_STREAMING:
c8f79162 3452 size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
2d792818
GP		 3453 chan->conn->mtu - L2CAP_EXT_HDR_SIZE -
3454 L2CAP_SDULEN_SIZE - L2CAP_FCS_SIZE);
c8f79162
AE		 3455 rfc.max_pdu_size = cpu_to_le16(size);
3456 chan->remote_mps = size;
f2fcfcd6 3457
c1360a1c 3458 set_bit(CONF_MODE_DONE, &chan->conf_state);
68ae6639 3459
2d792818
GP		 3460 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3461 (unsigned long) &rfc);
68ae6639 3462
f2fcfcd6
GP		 3463 break;
3464
3465 default:
5dee9e7c 3466 result = L2CAP_CONF_UNACCEPT;
5dee9e7c 3467
6464f35f 3468 memset(&rfc, 0, sizeof(rfc));
0c1bc5c6 3469 rfc.mode = chan->mode;
f2fcfcd6 3470 }
6464f35f 3471
f2fcfcd6 3472 if (result == L2CAP_CONF_SUCCESS)
c1360a1c 3473 set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
f2fcfcd6 3474 }
fe4128e0 3475 rsp->scid = cpu_to_le16(chan->dcid);
5dee9e7c 3476 rsp->result = cpu_to_le16(result);
59e54bd1 3477 rsp->flags = __constant_cpu_to_le16(0);
5dee9e7c
MH		 3478
3479 return ptr - data;
1da177e4
LT		 3480}
3481
2d792818
GP		 3482static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len,
3483 void *data, u16 *result)
f2fcfcd6 3484{
f2fcfcd6
GP		 3485 struct l2cap_conf_req *req = data;
3486 void *ptr = req->data;
3487 int type, olen;
3488 unsigned long val;
36e999a8 3489 struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
66af7aaf 3490 struct l2cap_conf_efs efs;
f2fcfcd6 3491
fe4128e0 3492 BT_DBG("chan %p, rsp %p, len %d, req %p", chan, rsp, len, data);
f2fcfcd6
GP		 3493
3494 while (len >= L2CAP_CONF_OPT_SIZE) {
3495 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
3496
3497 switch (type) {
3498 case L2CAP_CONF_MTU:
3499 if (val < L2CAP_DEFAULT_MIN_MTU) {
3500 *result = L2CAP_CONF_UNACCEPT;
0c1bc5c6 3501 chan->imtu = L2CAP_DEFAULT_MIN_MTU;
f2fcfcd6 3502 } else
0c1bc5c6
GP		 3503 chan->imtu = val;
3504 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu);
f2fcfcd6
GP		 3505 break;
3506
3507 case L2CAP_CONF_FLUSH_TO:
0c1bc5c6 3508 chan->flush_to = val;
f2fcfcd6 3509 l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
2d792818 3510 2, chan->flush_to);
f2fcfcd6
GP		 3511 break;
3512
3513 case L2CAP_CONF_RFC:
3514 if (olen == sizeof(rfc))
3515 memcpy(&rfc, (void *)val, olen);
3516
c1360a1c 3517 if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state) &&
2d792818 3518 rfc.mode != chan->mode)
f2fcfcd6
GP		 3519 return -ECONNREFUSED;
3520
47d1ec61 3521 chan->fcs = 0;
f2fcfcd6
GP		 3522
3523 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2d792818 3524 sizeof(rfc), (unsigned long) &rfc);
f2fcfcd6 3525 break;
6327eb98
AE		 3526
3527 case L2CAP_CONF_EWS:
c20f8e35 3528 chan->ack_win = min_t(u16, val, chan->ack_win);
3e6b3b95 3529 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
c20f8e35 3530 chan->tx_win);
6327eb98 3531 break;
66af7aaf
AE		 3532
3533 case L2CAP_CONF_EFS:
3534 if (olen == sizeof(efs))
3535 memcpy(&efs, (void *)val, olen);
3536
3537 if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
2d792818
GP		 3538 efs.stype != L2CAP_SERV_NOTRAFIC &&
3539 efs.stype != chan->local_stype)
66af7aaf
AE		 3540 return -ECONNREFUSED;
3541
2d792818
GP		 3542 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS, sizeof(efs),
3543 (unsigned long) &efs);
66af7aaf 3544 break;
cbabee78
AE		 3545
3546 case L2CAP_CONF_FCS:
3547 if (*result == L2CAP_CONF_PENDING)
3548 if (val == L2CAP_FCS_NONE)
f2592d3e 3549 set_bit(CONF_RECV_NO_FCS,
cbabee78
AE		 3550 &chan->conf_state);
3551 break;
f2fcfcd6
GP		 3552 }
3553 }
3554
0c1bc5c6 3555 if (chan->mode == L2CAP_MODE_BASIC && chan->mode != rfc.mode)
6c2ea7a8
GP		 3556 return -ECONNREFUSED;
3557
0c1bc5c6 3558 chan->mode = rfc.mode;
6c2ea7a8 3559
0e8b207e 3560 if (*result == L2CAP_CONF_SUCCESS || *result == L2CAP_CONF_PENDING) {
f2fcfcd6
GP		 3561 switch (rfc.mode) {
3562 case L2CAP_MODE_ERTM:
47d1ec61
GP		 3563 chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
3564 chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
3565 chan->mps = le16_to_cpu(rfc.max_pdu_size);
c20f8e35
MM		 3566 if (!test_bit(FLAG_EXT_CTRL, &chan->flags))
3567 chan->ack_win = min_t(u16, chan->ack_win,
3568 rfc.txwin_size);
66af7aaf
AE		 3569
3570 if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
3571 chan->local_msdu = le16_to_cpu(efs.msdu);
3572 chan->local_sdu_itime =
2d792818 3573 le32_to_cpu(efs.sdu_itime);
66af7aaf
AE		 3574 chan->local_acc_lat = le32_to_cpu(efs.acc_lat);
3575 chan->local_flush_to =
2d792818 3576 le32_to_cpu(efs.flush_to);
66af7aaf 3577 }
f2fcfcd6 3578 break;
66af7aaf 3579
f2fcfcd6 3580 case L2CAP_MODE_STREAMING:
47d1ec61 3581 chan->mps = le16_to_cpu(rfc.max_pdu_size);
f2fcfcd6
GP		 3582 }
3583 }
3584
fe4128e0 3585 req->dcid = cpu_to_le16(chan->dcid);
59e54bd1 3586 req->flags = __constant_cpu_to_le16(0);
f2fcfcd6
GP		 3587
3588 return ptr - data;
3589}
3590
2d792818
GP		 3591static int l2cap_build_conf_rsp(struct l2cap_chan *chan, void *data,
3592 u16 result, u16 flags)
1da177e4
LT		 3593{
3594 struct l2cap_conf_rsp *rsp = data;
3595 void *ptr = rsp->data;
1da177e4 3596
fe4128e0 3597 BT_DBG("chan %p", chan);
1da177e4 3598
fe4128e0 3599 rsp->scid = cpu_to_le16(chan->dcid);
5dee9e7c 3600 rsp->result = cpu_to_le16(result);
aca3192c 3601 rsp->flags = cpu_to_le16(flags);
1da177e4
LT		 3602
3603 return ptr - data;
3604}
3605
8c1d787b 3606void __l2cap_connect_rsp_defer(struct l2cap_chan *chan)
710f9b0a
GP		 3607{
3608 struct l2cap_conn_rsp rsp;
8c1d787b 3609 struct l2cap_conn *conn = chan->conn;
710f9b0a 3610 u8 buf[128];
439f34ac 3611 u8 rsp_code;
710f9b0a 3612
fe4128e0
GP		 3613 rsp.scid = cpu_to_le16(chan->dcid);
3614 rsp.dcid = cpu_to_le16(chan->scid);
ac73498c
AE		 3615 rsp.result = __constant_cpu_to_le16(L2CAP_CR_SUCCESS);
3616 rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
439f34ac
AE		 3617
3618 if (chan->hs_hcon)
3619 rsp_code = L2CAP_CREATE_CHAN_RSP;
3620 else
3621 rsp_code = L2CAP_CONN_RSP;
3622
3623 BT_DBG("chan %p rsp_code %u", chan, rsp_code);
3624
3625 l2cap_send_cmd(conn, chan->ident, rsp_code, sizeof(rsp), &rsp);
710f9b0a 3626
c1360a1c 3627 if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
710f9b0a
GP		 3628 return;
3629
710f9b0a 3630 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2d792818 3631 l2cap_build_conf_req(chan, buf), buf);
710f9b0a
GP		 3632 chan->num_conf_req++;
3633}
3634
47d1ec61 3635static void l2cap_conf_rfc_get(struct l2cap_chan *chan, void *rsp, int len)
7b1c0049 3636{
7b1c0049
GP		 3637 int type, olen;
3638 unsigned long val;
c20f8e35
MM		 3639 /* Use sane default values in case a misbehaving remote device
3640 * did not send an RFC or extended window size option.
3641 */
3642 u16 txwin_ext = chan->ack_win;
3643 struct l2cap_conf_rfc rfc = {
3644 .mode = chan->mode,
3645 .retrans_timeout = __constant_cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO),
3646 .monitor_timeout = __constant_cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO),
3647 .max_pdu_size = cpu_to_le16(chan->imtu),
3648 .txwin_size = min_t(u16, chan->ack_win, L2CAP_DEFAULT_TX_WINDOW),
3649 };
7b1c0049 3650
47d1ec61 3651 BT_DBG("chan %p, rsp %p, len %d", chan, rsp, len);
7b1c0049 3652
0c1bc5c6 3653 if ((chan->mode != L2CAP_MODE_ERTM) && (chan->mode != L2CAP_MODE_STREAMING))
7b1c0049
GP		 3654 return;
3655
3656 while (len >= L2CAP_CONF_OPT_SIZE) {
3657 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
3658
c20f8e35
MM		 3659 switch (type) {
3660 case L2CAP_CONF_RFC:
3661 if (olen == sizeof(rfc))
3662 memcpy(&rfc, (void *)val, olen);
8f321f85 3663 break;
c20f8e35
MM		 3664 case L2CAP_CONF_EWS:
3665 txwin_ext = val;
3666 break;
3667 }
7b1c0049
GP		 3668 }
3669
7b1c0049
GP		 3670 switch (rfc.mode) {
3671 case L2CAP_MODE_ERTM:
47d1ec61
GP		 3672 chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
3673 chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
c20f8e35
MM		 3674 chan->mps = le16_to_cpu(rfc.max_pdu_size);
3675 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
3676 chan->ack_win = min_t(u16, chan->ack_win, txwin_ext);
3677 else
3678 chan->ack_win = min_t(u16, chan->ack_win,
3679 rfc.txwin_size);
7b1c0049
GP		 3680 break;
3681 case L2CAP_MODE_STREAMING:
47d1ec61 3682 chan->mps = le16_to_cpu(rfc.max_pdu_size);
7b1c0049
GP		 3683 }
3684}
3685
2d792818 3686static inline int l2cap_command_rej(struct l2cap_conn *conn,
cb3b3152
JH		 3687 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
3688 u8 *data)
4e8402a3 3689{
e2fd318e 3690 struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data;
4e8402a3 3691
cb3b3152
JH		 3692 if (cmd_len < sizeof(*rej))
3693 return -EPROTO;
3694
e2fd318e 3695 if (rej->reason != L2CAP_REJ_NOT_UNDERSTOOD)
4e8402a3
MH		 3696 return 0;
3697
3698 if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
2d792818 3699 cmd->ident == conn->info_ident) {
17cd3f37 3700 cancel_delayed_work(&conn->info_timer);
984947dc
MH		 3701
3702 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
e1027a7c 3703 conn->info_ident = 0;
984947dc 3704
4e8402a3
MH		 3705 l2cap_conn_start(conn);
3706 }
3707
3708 return 0;
3709}
3710
1700915f
MM		 3711static struct l2cap_chan *l2cap_connect(struct l2cap_conn *conn,
3712 struct l2cap_cmd_hdr *cmd,
3713 u8 *data, u8 rsp_code, u8 amp_id)
1da177e4 3714{
1da177e4
LT		 3715 struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
3716 struct l2cap_conn_rsp rsp;
23691d75 3717 struct l2cap_chan *chan = NULL, *pchan;
d793fe8c 3718 struct sock *parent, *sk = NULL;
e7c29cb1 3719 int result, status = L2CAP_CS_NO_INFO;
1da177e4
LT		 3720
3721 u16 dcid = 0, scid = __le16_to_cpu(req->scid);
e7c29cb1 3722 __le16 psm = req->psm;
1da177e4 3723
097db76c 3724 BT_DBG("psm 0x%2.2x scid 0x%4.4x", __le16_to_cpu(psm), scid);
1da177e4
LT		 3725
3726 /* Check if we have socket listening on psm */
c2287681 3727 pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, conn->src, conn->dst);
23691d75 3728 if (!pchan) {
1da177e4
LT		 3729 result = L2CAP_CR_BAD_PSM;
3730 goto sendresp;
3731 }
3732
23691d75
GP		 3733 parent = pchan->sk;
3734
3df91ea2 3735 mutex_lock(&conn->chan_lock);
aa2ac881 3736 lock_sock(parent);
e0f0cb56 3737
e7c29cb1 3738 /* Check if the ACL is secure enough (if not SDP) */
2983fd68 3739 if (psm != __constant_cpu_to_le16(L2CAP_PSM_SDP) &&
2d792818 3740 !hci_conn_check_link_mode(conn->hcon)) {
9f5a0d7b 3741 conn->disc_reason = HCI_ERROR_AUTH_FAILURE;
e7c29cb1
MH		 3742 result = L2CAP_CR_SEC_BLOCK;
3743 goto response;
3744 }
3745
1da177e4
LT		 3746 result = L2CAP_CR_NO_MEM;
3747
2dfa1003
GP		 3748 /* Check if we already have channel with that dcid */
3749 if (__l2cap_get_chan_by_dcid(conn, scid))
3750 goto response;
3751
80b98027 3752 chan = pchan->ops->new_connection(pchan);
80808e43 3753 if (!chan)
1da177e4
LT		 3754 goto response;
3755
80808e43
GP		 3756 sk = chan->sk;
3757
1da177e4
LT		 3758 bacpy(&bt_sk(sk)->src, conn->src);
3759 bacpy(&bt_sk(sk)->dst, conn->dst);
fe4128e0
GP		 3760 chan->psm = psm;
3761 chan->dcid = scid;
1700915f 3762 chan->local_amp_id = amp_id;
1da177e4 3763
6be36555 3764 __l2cap_chan_add(conn, chan);
48454079 3765
fe4128e0 3766 dcid = chan->scid;
1da177e4 3767
c9b66675 3768 __set_chan_timer(chan, sk->sk_sndtimeo);
1da177e4 3769
fc7f8a7e 3770 chan->ident = cmd->ident;
1da177e4 3771
984947dc 3772 if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
d45fc423 3773 if (l2cap_chan_check_security(chan)) {
c5daa683 3774 if (test_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags)) {
0e587be7 3775 __l2cap_state_change(chan, BT_CONNECT2);
f66dc81f
MH		 3776 result = L2CAP_CR_PEND;
3777 status = L2CAP_CS_AUTHOR_PEND;
2dc4e510 3778 chan->ops->defer(chan);
f66dc81f 3779 } else {
1700915f
MM		 3780 /* Force pending result for AMP controllers.
3781 * The connection will succeed after the
3782 * physical link is up.
3783 */
3784 if (amp_id) {
3785 __l2cap_state_change(chan, BT_CONNECT2);
3786 result = L2CAP_CR_PEND;
3787 } else {
3788 __l2cap_state_change(chan, BT_CONFIG);
3789 result = L2CAP_CR_SUCCESS;
3790 }
f66dc81f
MH		 3791 status = L2CAP_CS_NO_INFO;
3792 }
79d554a6 3793 } else {
0e587be7 3794 __l2cap_state_change(chan, BT_CONNECT2);
79d554a6
MH		 3795 result = L2CAP_CR_PEND;
3796 status = L2CAP_CS_AUTHEN_PEND;
3797 }
3798 } else {
0e587be7 3799 __l2cap_state_change(chan, BT_CONNECT2);
79d554a6
MH		 3800 result = L2CAP_CR_PEND;
3801 status = L2CAP_CS_NO_INFO;
1da177e4
LT		 3802 }
3803
1da177e4 3804response:
aa2ac881 3805 release_sock(parent);
3df91ea2 3806 mutex_unlock(&conn->chan_lock);
1da177e4
LT		 3807
3808sendresp:
aca3192c
YH		 3809 rsp.scid = cpu_to_le16(scid);
3810 rsp.dcid = cpu_to_le16(dcid);
3811 rsp.result = cpu_to_le16(result);
3812 rsp.status = cpu_to_le16(status);
4c89b6aa 3813 l2cap_send_cmd(conn, cmd->ident, rsp_code, sizeof(rsp), &rsp);
79d554a6
MH		 3814
3815 if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
3816 struct l2cap_info_req info;
ac73498c 3817 info.type = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
79d554a6
MH		 3818
3819 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
3820 conn->info_ident = l2cap_get_ident(conn);
3821
ba13ccd9 3822 schedule_delayed_work(&conn->info_timer, L2CAP_INFO_TIMEOUT);
79d554a6 3823
2d792818
GP		 3824 l2cap_send_cmd(conn, conn->info_ident, L2CAP_INFO_REQ,
3825 sizeof(info), &info);
79d554a6
MH		 3826 }
3827
c1360a1c 3828 if (chan && !test_bit(CONF_REQ_SENT, &chan->conf_state) &&
2d792818 3829 result == L2CAP_CR_SUCCESS) {
e9aeb2dd 3830 u8 buf[128];
c1360a1c 3831 set_bit(CONF_REQ_SENT, &chan->conf_state);
e9aeb2dd 3832 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2d792818 3833 l2cap_build_conf_req(chan, buf), buf);
73ffa904 3834 chan->num_conf_req++;
e9aeb2dd 3835 }
1700915f
MM		 3836
3837 return chan;
4c89b6aa 3838}
e9aeb2dd 3839
4c89b6aa 3840static int l2cap_connect_req(struct l2cap_conn *conn,
cb3b3152 3841 struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
4c89b6aa 3842{
7b064eda
JK		 3843 struct hci_dev *hdev = conn->hcon->hdev;
3844 struct hci_conn *hcon = conn->hcon;
3845
cb3b3152
JH		 3846 if (cmd_len < sizeof(struct l2cap_conn_req))
3847 return -EPROTO;
3848
7b064eda
JK		 3849 hci_dev_lock(hdev);
3850 if (test_bit(HCI_MGMT, &hdev->dev_flags) &&
3851 !test_and_set_bit(HCI_CONN_MGMT_CONNECTED, &hcon->flags))
3852 mgmt_device_connected(hdev, &hcon->dst, hcon->type,
3853 hcon->dst_type, 0, NULL, 0,
3854 hcon->dev_class);
3855 hci_dev_unlock(hdev);
3856
300229f9 3857 l2cap_connect(conn, cmd, data, L2CAP_CONN_RSP, 0);
1da177e4
LT		 3858 return 0;
3859}
3860
5909cf30 3861static int l2cap_connect_create_rsp(struct l2cap_conn *conn,
cb3b3152
JH		 3862 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
3863 u8 *data)
1da177e4
LT		 3864{
3865 struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
3866 u16 scid, dcid, result, status;
48454079 3867 struct l2cap_chan *chan;
1da177e4 3868 u8 req[128];
3df91ea2 3869 int err;
1da177e4 3870
cb3b3152
JH		 3871 if (cmd_len < sizeof(*rsp))
3872 return -EPROTO;
3873
1da177e4
LT		 3874 scid = __le16_to_cpu(rsp->scid);
3875 dcid = __le16_to_cpu(rsp->dcid);
3876 result = __le16_to_cpu(rsp->result);
3877 status = __le16_to_cpu(rsp->status);
3878
1b009c98 3879 BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x",
2d792818 3880 dcid, scid, result, status);
1da177e4 3881
3df91ea2
AE		 3882 mutex_lock(&conn->chan_lock);
3883
1da177e4 3884 if (scid) {
3df91ea2
AE		 3885 chan = __l2cap_get_chan_by_scid(conn, scid);
3886 if (!chan) {
21870b52 3887 err = -EBADSLT;
3df91ea2
AE		 3888 goto unlock;
3889 }
1da177e4 3890 } else {
3df91ea2
AE		 3891 chan = __l2cap_get_chan_by_ident(conn, cmd->ident);
3892 if (!chan) {
21870b52 3893 err = -EBADSLT;
3df91ea2
AE		 3894 goto unlock;
3895 }
1da177e4
LT		 3896 }
3897
3df91ea2
AE		 3898 err = 0;
3899
6be36555 3900 l2cap_chan_lock(chan);
48454079 3901
1da177e4
LT		 3902 switch (result) {
3903 case L2CAP_CR_SUCCESS:
89bc500e 3904 l2cap_state_change(chan, BT_CONFIG);
fc7f8a7e 3905 chan->ident = 0;
fe4128e0 3906 chan->dcid = dcid;
c1360a1c 3907 clear_bit(CONF_CONNECT_PEND, &chan->conf_state);
6a8d3010 3908
c1360a1c 3909 if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
e9aeb2dd
GP		 3910 break;
3911
1da177e4 3912 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2d792818 3913 l2cap_build_conf_req(chan, req), req);
73ffa904 3914 chan->num_conf_req++;
1da177e4
LT		 3915 break;
3916
3917 case L2CAP_CR_PEND:
c1360a1c 3918 set_bit(CONF_CONNECT_PEND, &chan->conf_state);
1da177e4
LT		 3919 break;
3920
3921 default:
48454079 3922 l2cap_chan_del(chan, ECONNREFUSED);
1da177e4
LT		 3923 break;
3924 }
3925
6be36555 3926 l2cap_chan_unlock(chan);
3df91ea2
AE		 3927
3928unlock:
3929 mutex_unlock(&conn->chan_lock);
3930
3931 return err;
1da177e4
LT		 3932}
3933
47d1ec61 3934static inline void set_default_fcs(struct l2cap_chan *chan)
8c462b60
MM		 3935{
3936 /* FCS is enabled only in ERTM or streaming mode, if one or both
3937 * sides request it.
3938 */
0c1bc5c6 3939 if (chan->mode != L2CAP_MODE_ERTM && chan->mode != L2CAP_MODE_STREAMING)
47d1ec61 3940 chan->fcs = L2CAP_FCS_NONE;
f2592d3e 3941 else if (!test_bit(CONF_RECV_NO_FCS, &chan->conf_state))
47d1ec61 3942 chan->fcs = L2CAP_FCS_CRC16;
8c462b60
MM		 3943}
3944
29d8a590
AE		 3945static void l2cap_send_efs_conf_rsp(struct l2cap_chan *chan, void *data,
3946 u8 ident, u16 flags)
3947{
3948 struct l2cap_conn *conn = chan->conn;
3949
3950 BT_DBG("conn %p chan %p ident %d flags 0x%4.4x", conn, chan, ident,
3951 flags);
3952
3953 clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
3954 set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
3955
3956 l2cap_send_cmd(conn, ident, L2CAP_CONF_RSP,
3957 l2cap_build_conf_rsp(chan, data,
3958 L2CAP_CONF_SUCCESS, flags), data);
3959}
3960
2d792818
GP		 3961static inline int l2cap_config_req(struct l2cap_conn *conn,
3962 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
3963 u8 *data)
1da177e4
LT		 3964{
3965 struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
3966 u16 dcid, flags;
3967 u8 rsp[64];
48454079 3968 struct l2cap_chan *chan;
3c588192 3969 int len, err = 0;
1da177e4 3970
cb3b3152
JH		 3971 if (cmd_len < sizeof(*req))
3972 return -EPROTO;
3973
1da177e4
LT		 3974 dcid = __le16_to_cpu(req->dcid);
3975 flags = __le16_to_cpu(req->flags);
3976
3977 BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
3978
baa7e1fa 3979 chan = l2cap_get_chan_by_scid(conn, dcid);
48454079 3980 if (!chan)
21870b52 3981 return -EBADSLT;
1da177e4 3982
033b1142 3983 if (chan->state != BT_CONFIG && chan->state != BT_CONNECT2) {
e2fd318e
IK		 3984 struct l2cap_cmd_rej_cid rej;
3985
ac73498c 3986 rej.reason = __constant_cpu_to_le16(L2CAP_REJ_INVALID_CID);
e2fd318e
IK		 3987 rej.scid = cpu_to_le16(chan->scid);
3988 rej.dcid = cpu_to_le16(chan->dcid);
df6bd743 3989
df6bd743 3990 l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ,
2d792818 3991 sizeof(rej), &rej);
354f60a9 3992 goto unlock;
df6bd743 3993 }
354f60a9 3994
5dee9e7c 3995 /* Reject if config buffer is too small. */
88219a0f 3996 len = cmd_len - sizeof(*req);
cb3b3152 3997 if (chan->conf_len + len > sizeof(chan->conf_req)) {
5dee9e7c 3998 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2d792818
GP		 3999 l2cap_build_conf_rsp(chan, rsp,
4000 L2CAP_CONF_REJECT, flags), rsp);
5dee9e7c
MH		 4001 goto unlock;
4002 }
4003
4004 /* Store config. */
73ffa904
GP		 4005 memcpy(chan->conf_req + chan->conf_len, req->data, len);
4006 chan->conf_len += len;
1da177e4 4007
59e54bd1 4008 if (flags & L2CAP_CONF_FLAG_CONTINUATION) {
1da177e4
LT		 4009 /* Incomplete config. Send empty response. */
4010 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2d792818
GP		 4011 l2cap_build_conf_rsp(chan, rsp,
4012 L2CAP_CONF_SUCCESS, flags), rsp);
1da177e4
LT		 4013 goto unlock;
4014 }
4015
4016 /* Complete config. */
73ffa904 4017 len = l2cap_parse_conf_req(chan, rsp);
f2fcfcd6 4018 if (len < 0) {
5e4e3972 4019 l2cap_send_disconn_req(chan, ECONNRESET);
1da177e4 4020 goto unlock;
f2fcfcd6 4021 }
1da177e4 4022
1500109b 4023 chan->ident = cmd->ident;
5dee9e7c 4024 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
73ffa904 4025 chan->num_conf_rsp++;
5dee9e7c 4026
5dee9e7c 4027 /* Reset config buffer. */
73ffa904 4028 chan->conf_len = 0;
5dee9e7c 4029
c1360a1c 4030 if (!test_bit(CONF_OUTPUT_DONE, &chan->conf_state))
876d9484
MH		 4031 goto unlock;
4032
c1360a1c 4033 if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
47d1ec61 4034 set_default_fcs(chan);
fcc203c3 4035
105bdf9e
MM		 4036 if (chan->mode == L2CAP_MODE_ERTM ||
4037 chan->mode == L2CAP_MODE_STREAMING)
3c588192
MM		 4038 err = l2cap_ertm_init(chan);
4039
4040 if (err < 0)
5e4e3972 4041 l2cap_send_disconn_req(chan, -err);
3c588192
MM		 4042 else
4043 l2cap_chan_ready(chan);
0565c1c2 4044
876d9484
MH		 4045 goto unlock;
4046 }
4047
c1360a1c 4048 if (!test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) {
79d554a6 4049 u8 buf[64];
1da177e4 4050 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2d792818 4051 l2cap_build_conf_req(chan, buf), buf);
73ffa904 4052 chan->num_conf_req++;
1da177e4
LT		 4053 }
4054
0e8b207e
AE		 4055 /* Got Conf Rsp PENDING from remote side and asume we sent
4056 Conf Rsp PENDING in the code above */
4057 if (test_bit(CONF_REM_CONF_PEND, &chan->conf_state) &&
29d8a590 4058 test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
0e8b207e
AE		 4059
4060 /* check compatibility */
4061
79de886d 4062 /* Send rsp for BR/EDR channel */
f351bc72 4063 if (!chan->hs_hcon)
79de886d
AE		 4064 l2cap_send_efs_conf_rsp(chan, rsp, cmd->ident, flags);
4065 else
4066 chan->ident = cmd->ident;
0e8b207e
AE		 4067 }
4068
1da177e4 4069unlock:
6be36555 4070 l2cap_chan_unlock(chan);
3c588192 4071 return err;
1da177e4
LT		 4072}
4073
2d792818 4074static inline int l2cap_config_rsp(struct l2cap_conn *conn,
cb3b3152
JH		 4075 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4076 u8 *data)
1da177e4
LT		 4077{
4078 struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
4079 u16 scid, flags, result;
48454079 4080 struct l2cap_chan *chan;
cb3b3152 4081 int len = cmd_len - sizeof(*rsp);
3c588192 4082 int err = 0;
1da177e4 4083
cb3b3152
JH		 4084 if (cmd_len < sizeof(*rsp))
4085 return -EPROTO;
4086
1da177e4
LT		 4087 scid = __le16_to_cpu(rsp->scid);
4088 flags = __le16_to_cpu(rsp->flags);
4089 result = __le16_to_cpu(rsp->result);
4090
61386cba
AE		 4091 BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x len %d", scid, flags,
4092 result, len);
1da177e4 4093
baa7e1fa 4094 chan = l2cap_get_chan_by_scid(conn, scid);
48454079 4095 if (!chan)
1da177e4
LT		 4096 return 0;
4097
4098 switch (result) {
4099 case L2CAP_CONF_SUCCESS:
47d1ec61 4100 l2cap_conf_rfc_get(chan, rsp->data, len);
0e8b207e 4101 clear_bit(CONF_REM_CONF_PEND, &chan->conf_state);
1da177e4
LT		 4102 break;
4103
0e8b207e
AE		 4104 case L2CAP_CONF_PENDING:
4105 set_bit(CONF_REM_CONF_PEND, &chan->conf_state);
4106
4107 if (test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
4108 char buf[64];
4109
4110 len = l2cap_parse_conf_rsp(chan, rsp->data, len,
2d792818 4111 buf, &result);
0e8b207e 4112 if (len < 0) {
5e4e3972 4113 l2cap_send_disconn_req(chan, ECONNRESET);
0e8b207e
AE		 4114 goto done;
4115 }
4116
f351bc72 4117 if (!chan->hs_hcon) {
79de886d
AE		 4118 l2cap_send_efs_conf_rsp(chan, buf, cmd->ident,
4119 0);
5ce66b59
AE		 4120 } else {
4121 if (l2cap_check_efs(chan)) {
4122 amp_create_logical_link(chan);
4123 chan->ident = cmd->ident;
4124 }
4125 }
0e8b207e
AE		 4126 }
4127 goto done;
4128
1da177e4 4129 case L2CAP_CONF_UNACCEPT:
73ffa904 4130 if (chan->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
f2fcfcd6
GP		 4131 char req[64];
4132
c2c77ec8 4133 if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) {
5e4e3972 4134 l2cap_send_disconn_req(chan, ECONNRESET);
c2c77ec8
AE		 4135 goto done;
4136 }
4137
f2fcfcd6
GP		 4138 /* throw out any old stored conf requests */
4139 result = L2CAP_CONF_SUCCESS;
b4450035 4140 len = l2cap_parse_conf_rsp(chan, rsp->data, len,
2d792818 4141 req, &result);
f2fcfcd6 4142 if (len < 0) {
5e4e3972 4143 l2cap_send_disconn_req(chan, ECONNRESET);
f2fcfcd6
GP		 4144 goto done;
4145 }
4146
4147 l2cap_send_cmd(conn, l2cap_get_ident(conn),
2d792818 4148 L2CAP_CONF_REQ, len, req);
73ffa904 4149 chan->num_conf_req++;
f2fcfcd6
GP		 4150 if (result != L2CAP_CONF_SUCCESS)
4151 goto done;
4152 break;
1da177e4
LT		 4153 }
4154
8e87d142 4155 default:
6be36555 4156 l2cap_chan_set_err(chan, ECONNRESET);
2e0052e4 4157
ba13ccd9 4158 __set_chan_timer(chan, L2CAP_DISC_REJ_TIMEOUT);
5e4e3972 4159 l2cap_send_disconn_req(chan, ECONNRESET);
1da177e4
LT		 4160 goto done;
4161 }
4162
59e54bd1 4163 if (flags & L2CAP_CONF_FLAG_CONTINUATION)
1da177e4
LT		 4164 goto done;
4165
c1360a1c 4166 set_bit(CONF_INPUT_DONE, &chan->conf_state);
1da177e4 4167
c1360a1c 4168 if (test_bit(CONF_OUTPUT_DONE, &chan->conf_state)) {
47d1ec61 4169 set_default_fcs(chan);
fcc203c3 4170
105bdf9e
MM		 4171 if (chan->mode == L2CAP_MODE_ERTM ||
4172 chan->mode == L2CAP_MODE_STREAMING)
3c588192 4173 err = l2cap_ertm_init(chan);
0565c1c2 4174
3c588192 4175 if (err < 0)
5e4e3972 4176 l2cap_send_disconn_req(chan, -err);
3c588192
MM		 4177 else
4178 l2cap_chan_ready(chan);
1da177e4
LT		 4179 }
4180
4181done:
6be36555 4182 l2cap_chan_unlock(chan);
3c588192 4183 return err;
1da177e4
LT		 4184}
4185
2d792818 4186static inline int l2cap_disconnect_req(struct l2cap_conn *conn,
cb3b3152
JH		 4187 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4188 u8 *data)
1da177e4
LT		 4189{
4190 struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
4191 struct l2cap_disconn_rsp rsp;
4192 u16 dcid, scid;
48454079 4193 struct l2cap_chan *chan;
1da177e4
LT		 4194 struct sock *sk;
4195
cb3b3152
JH		 4196 if (cmd_len != sizeof(*req))
4197 return -EPROTO;
4198
1da177e4
LT		 4199 scid = __le16_to_cpu(req->scid);
4200 dcid = __le16_to_cpu(req->dcid);
4201
4202 BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
4203
3df91ea2
AE		 4204 mutex_lock(&conn->chan_lock);
4205
4206 chan = __l2cap_get_chan_by_scid(conn, dcid);
4207 if (!chan) {
4208 mutex_unlock(&conn->chan_lock);
c4ea249f 4209 return -EBADSLT;
3df91ea2 4210 }
1da177e4 4211
6be36555
AE		 4212 l2cap_chan_lock(chan);
4213
48454079
GP		 4214 sk = chan->sk;
4215
fe4128e0
GP		 4216 rsp.dcid = cpu_to_le16(chan->scid);
4217 rsp.scid = cpu_to_le16(chan->dcid);
1da177e4
LT		 4218 l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
4219
6be36555 4220 lock_sock(sk);
1da177e4 4221 sk->sk_shutdown = SHUTDOWN_MASK;
6be36555 4222 release_sock(sk);
1da177e4 4223
61d6ef3e 4224 l2cap_chan_hold(chan);
48454079 4225 l2cap_chan_del(chan, ECONNRESET);
6be36555
AE		 4226
4227 l2cap_chan_unlock(chan);
1da177e4 4228
80b98027 4229 chan->ops->close(chan);
61d6ef3e 4230 l2cap_chan_put(chan);
3df91ea2
AE		 4231
4232 mutex_unlock(&conn->chan_lock);
4233
1da177e4
LT		 4234 return 0;
4235}
4236
2d792818 4237static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn,
cb3b3152
JH		 4238 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4239 u8 *data)
1da177e4
LT		 4240{
4241 struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
4242 u16 dcid, scid;
48454079 4243 struct l2cap_chan *chan;
1da177e4 4244
cb3b3152
JH		 4245 if (cmd_len != sizeof(*rsp))
4246 return -EPROTO;
4247
1da177e4
LT		 4248 scid = __le16_to_cpu(rsp->scid);
4249 dcid = __le16_to_cpu(rsp->dcid);
4250
4251 BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
4252
3df91ea2
AE		 4253 mutex_lock(&conn->chan_lock);
4254
4255 chan = __l2cap_get_chan_by_scid(conn, scid);
4256 if (!chan) {
4257 mutex_unlock(&conn->chan_lock);
1da177e4 4258 return 0;
3df91ea2 4259 }
1da177e4 4260
6be36555 4261 l2cap_chan_lock(chan);
48454079 4262
61d6ef3e 4263 l2cap_chan_hold(chan);
48454079 4264 l2cap_chan_del(chan, 0);
6be36555
AE		 4265
4266 l2cap_chan_unlock(chan);
1da177e4 4267
80b98027 4268 chan->ops->close(chan);
61d6ef3e 4269 l2cap_chan_put(chan);
3df91ea2
AE		 4270
4271 mutex_unlock(&conn->chan_lock);
4272
1da177e4
LT		 4273 return 0;
4274}
4275
2d792818 4276static inline int l2cap_information_req(struct l2cap_conn *conn,
cb3b3152
JH		 4277 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4278 u8 *data)
1da177e4
LT		 4279{
4280 struct l2cap_info_req *req = (struct l2cap_info_req *) data;
1da177e4
LT		 4281 u16 type;
4282
cb3b3152
JH		 4283 if (cmd_len != sizeof(*req))
4284 return -EPROTO;
4285
1da177e4
LT		 4286 type = __le16_to_cpu(req->type);
4287
4288 BT_DBG("type 0x%4.4x", type);
4289
f0709e03
MH		 4290 if (type == L2CAP_IT_FEAT_MASK) {
4291 u8 buf[8];
44dd46de 4292 u32 feat_mask = l2cap_feat_mask;
f0709e03 4293 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
ac73498c
AE		 4294 rsp->type = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
4295 rsp->result = __constant_cpu_to_le16(L2CAP_IR_SUCCESS);
d1c4a17d 4296 if (!disable_ertm)
fcc203c3 4297 feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
2d792818 4298 | L2CAP_FEAT_FCS;
a5fd6f30 4299 if (enable_hs)
6327eb98 4300 feat_mask |= L2CAP_FEAT_EXT_FLOW
2d792818 4301 | L2CAP_FEAT_EXT_WINDOW;
a5fd6f30 4302
1b7bf4ed 4303 put_unaligned_le32(feat_mask, rsp->data);
2d792818
GP		 4304 l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf),
4305 buf);
e1027a7c
MH		 4306 } else if (type == L2CAP_IT_FIXED_CHAN) {
4307 u8 buf[12];
4308 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
50a147cd
MM		 4309
4310 if (enable_hs)
4311 l2cap_fixed_chan[0] |= L2CAP_FC_A2MP;
4312 else
4313 l2cap_fixed_chan[0] &= ~L2CAP_FC_A2MP;
4314
ac73498c
AE		 4315 rsp->type = __constant_cpu_to_le16(L2CAP_IT_FIXED_CHAN);
4316 rsp->result = __constant_cpu_to_le16(L2CAP_IR_SUCCESS);
c6337ea6 4317 memcpy(rsp->data, l2cap_fixed_chan, sizeof(l2cap_fixed_chan));
2d792818
GP		 4318 l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf),
4319 buf);
f0709e03
MH		 4320 } else {
4321 struct l2cap_info_rsp rsp;
4322 rsp.type = cpu_to_le16(type);
ac73498c 4323 rsp.result = __constant_cpu_to_le16(L2CAP_IR_NOTSUPP);
2d792818
GP		 4324 l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(rsp),
4325 &rsp);
f0709e03 4326 }
1da177e4
LT		 4327
4328 return 0;
4329}
4330
2d792818 4331static inline int l2cap_information_rsp(struct l2cap_conn *conn,
cb3b3152
JH		 4332 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4333 u8 *data)
1da177e4
LT		 4334{
4335 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
4336 u16 type, result;
4337
3f6fa3d4 4338 if (cmd_len < sizeof(*rsp))
cb3b3152
JH		 4339 return -EPROTO;
4340
1da177e4
LT		 4341 type = __le16_to_cpu(rsp->type);
4342 result = __le16_to_cpu(rsp->result);
4343
4344 BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
4345
e90165be
AE		 4346 /* L2CAP Info req/rsp are unbound to channels, add extra checks */
4347 if (cmd->ident != conn->info_ident ||
2d792818 4348 conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)
e90165be
AE		 4349 return 0;
4350
17cd3f37 4351 cancel_delayed_work(&conn->info_timer);
4e8402a3 4352
adb08ede
VT		 4353 if (result != L2CAP_IR_SUCCESS) {
4354 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4355 conn->info_ident = 0;
4356
4357 l2cap_conn_start(conn);
4358
4359 return 0;
4360 }
4361
978c93b9
AE		 4362 switch (type) {
4363 case L2CAP_IT_FEAT_MASK:
83985319 4364 conn->feat_mask = get_unaligned_le32(rsp->data);
4e8402a3 4365
47ec1dcd 4366 if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
e1027a7c 4367 struct l2cap_info_req req;
ac73498c 4368 req.type = __constant_cpu_to_le16(L2CAP_IT_FIXED_CHAN);
e1027a7c
MH		 4369
4370 conn->info_ident = l2cap_get_ident(conn);
4371
4372 l2cap_send_cmd(conn, conn->info_ident,
2d792818 4373 L2CAP_INFO_REQ, sizeof(req), &req);
e1027a7c
MH		 4374 } else {
4375 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4376 conn->info_ident = 0;
4377
4378 l2cap_conn_start(conn);
4379 }
978c93b9
AE		 4380 break;
4381
4382 case L2CAP_IT_FIXED_CHAN:
4383 conn->fixed_chan_mask = rsp->data[0];
984947dc 4384 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
e1027a7c 4385 conn->info_ident = 0;
984947dc
MH		 4386
4387 l2cap_conn_start(conn);
978c93b9 4388 break;
984947dc 4389 }
4e8402a3 4390
1da177e4
LT		 4391 return 0;
4392}
4393
1700915f
MM		 4394static int l2cap_create_channel_req(struct l2cap_conn *conn,
4395 struct l2cap_cmd_hdr *cmd,
4396 u16 cmd_len, void *data)
f94ff6ff
MM		 4397{
4398 struct l2cap_create_chan_req *req = data;
6e1df6a6 4399 struct l2cap_create_chan_rsp rsp;
1700915f 4400 struct l2cap_chan *chan;
6e1df6a6 4401 struct hci_dev *hdev;
f94ff6ff
MM		 4402 u16 psm, scid;
4403
4404 if (cmd_len != sizeof(*req))
4405 return -EPROTO;
4406
4407 if (!enable_hs)
4408 return -EINVAL;
4409
4410 psm = le16_to_cpu(req->psm);
4411 scid = le16_to_cpu(req->scid);
4412
ad0ac6ca 4413 BT_DBG("psm 0x%2.2x, scid 0x%4.4x, amp_id %d", psm, scid, req->amp_id);
f94ff6ff 4414
6e1df6a6
AE		 4415 /* For controller id 0 make BR/EDR connection */
4416 if (req->amp_id == HCI_BREDR_ID) {
4417 l2cap_connect(conn, cmd, data, L2CAP_CREATE_CHAN_RSP,
4418 req->amp_id);
4419 return 0;
4420 }
1700915f 4421
6e1df6a6
AE		 4422 /* Validate AMP controller id */
4423 hdev = hci_dev_get(req->amp_id);
4424 if (!hdev)
4425 goto error;
1700915f 4426
6e1df6a6
AE		 4427 if (hdev->dev_type != HCI_AMP || !test_bit(HCI_UP, &hdev->flags)) {
4428 hci_dev_put(hdev);
4429 goto error;
4430 }
1700915f 4431
6e1df6a6
AE		 4432 chan = l2cap_connect(conn, cmd, data, L2CAP_CREATE_CHAN_RSP,
4433 req->amp_id);
4434 if (chan) {
4435 struct amp_mgr *mgr = conn->hcon->amp_mgr;
4436 struct hci_conn *hs_hcon;
1700915f 4437
6e1df6a6
AE		 4438 hs_hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK, conn->dst);
4439 if (!hs_hcon) {
4440 hci_dev_put(hdev);
21870b52 4441 return -EBADSLT;
1700915f
MM		 4442 }
4443
6e1df6a6
AE		 4444 BT_DBG("mgr %p bredr_chan %p hs_hcon %p", mgr, chan, hs_hcon);
4445
6e1df6a6
AE		 4446 mgr->bredr_chan = chan;
4447 chan->hs_hcon = hs_hcon;
fd45bf4c 4448 chan->fcs = L2CAP_FCS_NONE;
6e1df6a6 4449 conn->mtu = hdev->block_mtu;
1700915f 4450 }
f94ff6ff 4451
6e1df6a6 4452 hci_dev_put(hdev);
f94ff6ff
MM		 4453
4454 return 0;
6e1df6a6
AE		 4455
4456error:
4457 rsp.dcid = 0;
4458 rsp.scid = cpu_to_le16(scid);
4459 rsp.result = __constant_cpu_to_le16(L2CAP_CR_BAD_AMP);
4460 rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
4461
4462 l2cap_send_cmd(conn, cmd->ident, L2CAP_CREATE_CHAN_RSP,
4463 sizeof(rsp), &rsp);
4464
dc280801 4465 return 0;
f94ff6ff
MM		 4466}
4467
8eb200bd
MM		 4468static void l2cap_send_move_chan_req(struct l2cap_chan *chan, u8 dest_amp_id)
4469{
4470 struct l2cap_move_chan_req req;
4471 u8 ident;
4472
4473 BT_DBG("chan %p, dest_amp_id %d", chan, dest_amp_id);
4474
4475 ident = l2cap_get_ident(chan->conn);
4476 chan->ident = ident;
4477
4478 req.icid = cpu_to_le16(chan->scid);
4479 req.dest_amp_id = dest_amp_id;
4480
4481 l2cap_send_cmd(chan->conn, ident, L2CAP_MOVE_CHAN_REQ, sizeof(req),
4482 &req);
4483
4484 __set_chan_timer(chan, L2CAP_MOVE_TIMEOUT);
4485}
4486
1500109b 4487static void l2cap_send_move_chan_rsp(struct l2cap_chan *chan, u16 result)
8d5a04a1
MM		 4488{
4489 struct l2cap_move_chan_rsp rsp;
4490
1500109b 4491 BT_DBG("chan %p, result 0x%4.4x", chan, result);
8d5a04a1 4492
1500109b 4493 rsp.icid = cpu_to_le16(chan->dcid);
8d5a04a1
MM		 4494 rsp.result = cpu_to_le16(result);
4495
1500109b
MM		 4496 l2cap_send_cmd(chan->conn, chan->ident, L2CAP_MOVE_CHAN_RSP,
4497 sizeof(rsp), &rsp);
8d5a04a1
MM		 4498}
4499
5b155ef9 4500static void l2cap_send_move_chan_cfm(struct l2cap_chan *chan, u16 result)
8d5a04a1
MM		 4501{
4502 struct l2cap_move_chan_cfm cfm;
8d5a04a1 4503
5b155ef9 4504 BT_DBG("chan %p, result 0x%4.4x", chan, result);
8d5a04a1 4505
5b155ef9 4506 chan->ident = l2cap_get_ident(chan->conn);
8d5a04a1 4507
5b155ef9 4508 cfm.icid = cpu_to_le16(chan->scid);
8d5a04a1
MM		 4509 cfm.result = cpu_to_le16(result);
4510
5b155ef9
MM		 4511 l2cap_send_cmd(chan->conn, chan->ident, L2CAP_MOVE_CHAN_CFM,
4512 sizeof(cfm), &cfm);
4513
4514 __set_chan_timer(chan, L2CAP_MOVE_TIMEOUT);
4515}
4516
4517static void l2cap_send_move_chan_cfm_icid(struct l2cap_conn *conn, u16 icid)
4518{
4519 struct l2cap_move_chan_cfm cfm;
4520
4521 BT_DBG("conn %p, icid 0x%4.4x", conn, icid);
4522
4523 cfm.icid = cpu_to_le16(icid);
4524 cfm.result = __constant_cpu_to_le16(L2CAP_MC_UNCONFIRMED);
4525
4526 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_MOVE_CHAN_CFM,
4527 sizeof(cfm), &cfm);
8d5a04a1
MM		 4528}
4529
4530static void l2cap_send_move_chan_cfm_rsp(struct l2cap_conn *conn, u8 ident,
ad0ac6ca 4531 u16 icid)
8d5a04a1
MM		 4532{
4533 struct l2cap_move_chan_cfm_rsp rsp;
4534
ad0ac6ca 4535 BT_DBG("icid 0x%4.4x", icid);
8d5a04a1
MM		 4536
4537 rsp.icid = cpu_to_le16(icid);
4538 l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM_RSP, sizeof(rsp), &rsp);
4539}
4540
5f3847a4
MM		 4541static void __release_logical_link(struct l2cap_chan *chan)
4542{
4543 chan->hs_hchan = NULL;
4544 chan->hs_hcon = NULL;
4545
4546 /* Placeholder - release the logical link */
4547}
4548
1500109b
MM		 4549static void l2cap_logical_fail(struct l2cap_chan *chan)
4550{
4551 /* Logical link setup failed */
4552 if (chan->state != BT_CONNECTED) {
4553 /* Create channel failure, disconnect */
5e4e3972 4554 l2cap_send_disconn_req(chan, ECONNRESET);
1500109b
MM		 4555 return;
4556 }
4557
4558 switch (chan->move_role) {
4559 case L2CAP_MOVE_ROLE_RESPONDER:
4560 l2cap_move_done(chan);
4561 l2cap_send_move_chan_rsp(chan, L2CAP_MR_NOT_SUPP);
4562 break;
4563 case L2CAP_MOVE_ROLE_INITIATOR:
4564 if (chan->move_state == L2CAP_MOVE_WAIT_LOGICAL_COMP ||
4565 chan->move_state == L2CAP_MOVE_WAIT_LOGICAL_CFM) {
4566 /* Remote has only sent pending or
4567 * success responses, clean up
4568 */
4569 l2cap_move_done(chan);
4570 }
4571
4572 /* Other amp move states imply that the move
4573 * has already aborted
4574 */
4575 l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
4576 break;
4577 }
4578}
4579
4580static void l2cap_logical_finish_create(struct l2cap_chan *chan,
4581 struct hci_chan *hchan)
4582{
4583 struct l2cap_conf_rsp rsp;
1500109b 4584
336178a3 4585 chan->hs_hchan = hchan;
1500109b
MM		 4586 chan->hs_hcon->l2cap_data = chan->conn;
4587
35ba9561 4588 l2cap_send_efs_conf_rsp(chan, &rsp, chan->ident, 0);
1500109b
MM		 4589
4590 if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
fe79c6fe 4591 int err;
1500109b
MM		 4592
4593 set_default_fcs(chan);
4594
4595 err = l2cap_ertm_init(chan);
4596 if (err < 0)
5e4e3972 4597 l2cap_send_disconn_req(chan, -err);
1500109b
MM		 4598 else
4599 l2cap_chan_ready(chan);
4600 }
4601}
4602
4603static void l2cap_logical_finish_move(struct l2cap_chan *chan,
4604 struct hci_chan *hchan)
4605{
4606 chan->hs_hcon = hchan->conn;
4607 chan->hs_hcon->l2cap_data = chan->conn;
4608
4609 BT_DBG("move_state %d", chan->move_state);
4610
4611 switch (chan->move_state) {
4612 case L2CAP_MOVE_WAIT_LOGICAL_COMP:
4613 /* Move confirm will be sent after a success
4614 * response is received
4615 */
4616 chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
4617 break;
4618 case L2CAP_MOVE_WAIT_LOGICAL_CFM:
4619 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
4620 chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
4621 } else if (chan->move_role == L2CAP_MOVE_ROLE_INITIATOR) {
4622 chan->move_state = L2CAP_MOVE_WAIT_CONFIRM_RSP;
4623 l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
4624 } else if (chan->move_role == L2CAP_MOVE_ROLE_RESPONDER) {
4625 chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
4626 l2cap_send_move_chan_rsp(chan, L2CAP_MR_SUCCESS);
4627 }
4628 break;
4629 default:
4630 /* Move was not in expected state, free the channel */
4631 __release_logical_link(chan);
4632
4633 chan->move_state = L2CAP_MOVE_STABLE;
4634 }
4635}
4636
4637/* Call with chan locked */
27695fb4
AE		 4638void l2cap_logical_cfm(struct l2cap_chan *chan, struct hci_chan *hchan,
4639 u8 status)
5b155ef9 4640{
1500109b
MM		 4641 BT_DBG("chan %p, hchan %p, status %d", chan, hchan, status);
4642
4643 if (status) {
4644 l2cap_logical_fail(chan);
4645 __release_logical_link(chan);
4646 return;
4647 }
4648
4649 if (chan->state != BT_CONNECTED) {
4650 /* Ignore logical link if channel is on BR/EDR */
4651 if (chan->local_amp_id)
4652 l2cap_logical_finish_create(chan, hchan);
4653 } else {
4654 l2cap_logical_finish_move(chan, hchan);
4655 }
5b155ef9
MM		 4656}
4657
3f7a56c4
MM		 4658void l2cap_move_start(struct l2cap_chan *chan)
4659{
4660 BT_DBG("chan %p", chan);
4661
4662 if (chan->local_amp_id == HCI_BREDR_ID) {
4663 if (chan->chan_policy != BT_CHANNEL_POLICY_AMP_PREFERRED)
4664 return;
4665 chan->move_role = L2CAP_MOVE_ROLE_INITIATOR;
4666 chan->move_state = L2CAP_MOVE_WAIT_PREPARE;
4667 /* Placeholder - start physical link setup */
4668 } else {
4669 chan->move_role = L2CAP_MOVE_ROLE_INITIATOR;
4670 chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
4671 chan->move_id = 0;
4672 l2cap_move_setup(chan);
4673 l2cap_send_move_chan_req(chan, 0);
4674 }
4675}
4676
8eb200bd
MM		 4677static void l2cap_do_create(struct l2cap_chan *chan, int result,
4678 u8 local_amp_id, u8 remote_amp_id)
4679{
62748ca1
AE		 4680 BT_DBG("chan %p state %s %u -> %u", chan, state_to_string(chan->state),
4681 local_amp_id, remote_amp_id);
4682
12d6cc60
AE		 4683 chan->fcs = L2CAP_FCS_NONE;
4684
62748ca1
AE		 4685 /* Outgoing channel on AMP */
4686 if (chan->state == BT_CONNECT) {
4687 if (result == L2CAP_CR_SUCCESS) {
4688 chan->local_amp_id = local_amp_id;
4689 l2cap_send_create_chan_req(chan, remote_amp_id);
4690 } else {
4691 /* Revert to BR/EDR connect */
4692 l2cap_send_conn_req(chan);
4693 }
4694
4695 return;
4696 }
4697
4698 /* Incoming channel on AMP */
4699 if (__l2cap_no_conn_pending(chan)) {
8eb200bd
MM		 4700 struct l2cap_conn_rsp rsp;
4701 char buf[128];
4702 rsp.scid = cpu_to_le16(chan->dcid);
4703 rsp.dcid = cpu_to_le16(chan->scid);
4704
8eb200bd
MM		 4705 if (result == L2CAP_CR_SUCCESS) {
4706 /* Send successful response */
62cd50e2
AE		 4707 rsp.result = __constant_cpu_to_le16(L2CAP_CR_SUCCESS);
4708 rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
8eb200bd
MM		 4709 } else {
4710 /* Send negative response */
62cd50e2
AE		 4711 rsp.result = __constant_cpu_to_le16(L2CAP_CR_NO_MEM);
4712 rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
8eb200bd
MM		 4713 }
4714
4715 l2cap_send_cmd(chan->conn, chan->ident, L2CAP_CREATE_CHAN_RSP,
4716 sizeof(rsp), &rsp);
4717
4718 if (result == L2CAP_CR_SUCCESS) {
4719 __l2cap_state_change(chan, BT_CONFIG);
4720 set_bit(CONF_REQ_SENT, &chan->conf_state);
4721 l2cap_send_cmd(chan->conn, l2cap_get_ident(chan->conn),
4722 L2CAP_CONF_REQ,
4723 l2cap_build_conf_req(chan, buf), buf);
4724 chan->num_conf_req++;
4725 }
8eb200bd
MM		 4726 }
4727}
4728
4729static void l2cap_do_move_initiate(struct l2cap_chan *chan, u8 local_amp_id,
4730 u8 remote_amp_id)
4731{
4732 l2cap_move_setup(chan);
4733 chan->move_id = local_amp_id;
4734 chan->move_state = L2CAP_MOVE_WAIT_RSP;
4735
4736 l2cap_send_move_chan_req(chan, remote_amp_id);
4737}
4738
4739static void l2cap_do_move_respond(struct l2cap_chan *chan, int result)
4740{
4741 struct hci_chan *hchan = NULL;
4742
4743 /* Placeholder - get hci_chan for logical link */
4744
4745 if (hchan) {
4746 if (hchan->state == BT_CONNECTED) {
4747 /* Logical link is ready to go */
4748 chan->hs_hcon = hchan->conn;
4749 chan->hs_hcon->l2cap_data = chan->conn;
4750 chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
4751 l2cap_send_move_chan_rsp(chan, L2CAP_MR_SUCCESS);
4752
4753 l2cap_logical_cfm(chan, hchan, L2CAP_MR_SUCCESS);
4754 } else {
4755 /* Wait for logical link to be ready */
4756 chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
4757 }
4758 } else {
4759 /* Logical link not available */
4760 l2cap_send_move_chan_rsp(chan, L2CAP_MR_NOT_ALLOWED);
4761 }
4762}
4763
4764static void l2cap_do_move_cancel(struct l2cap_chan *chan, int result)
4765{
4766 if (chan->move_role == L2CAP_MOVE_ROLE_RESPONDER) {
4767 u8 rsp_result;
4768 if (result == -EINVAL)
4769 rsp_result = L2CAP_MR_BAD_ID;
4770 else
4771 rsp_result = L2CAP_MR_NOT_ALLOWED;
4772
4773 l2cap_send_move_chan_rsp(chan, rsp_result);
4774 }
4775
4776 chan->move_role = L2CAP_MOVE_ROLE_NONE;
4777 chan->move_state = L2CAP_MOVE_STABLE;
4778
4779 /* Restart data transmission */
4780 l2cap_ertm_send(chan);
4781}
4782
a514b17f
AE		 4783/* Invoke with locked chan */
4784void __l2cap_physical_cfm(struct l2cap_chan *chan, int result)
8eb200bd 4785{
770bfefa 4786 u8 local_amp_id = chan->local_amp_id;
fffadc08 4787 u8 remote_amp_id = chan->remote_amp_id;
770bfefa 4788
8eb200bd
MM		 4789 BT_DBG("chan %p, result %d, local_amp_id %d, remote_amp_id %d",
4790 chan, result, local_amp_id, remote_amp_id);
4791
8eb200bd
MM		 4792 if (chan->state == BT_DISCONN || chan->state == BT_CLOSED) {
4793 l2cap_chan_unlock(chan);
4794 return;
4795 }
4796
4797 if (chan->state != BT_CONNECTED) {
4798 l2cap_do_create(chan, result, local_amp_id, remote_amp_id);
4799 } else if (result != L2CAP_MR_SUCCESS) {
4800 l2cap_do_move_cancel(chan, result);
4801 } else {
4802 switch (chan->move_role) {
4803 case L2CAP_MOVE_ROLE_INITIATOR:
4804 l2cap_do_move_initiate(chan, local_amp_id,
4805 remote_amp_id);
4806 break;
4807 case L2CAP_MOVE_ROLE_RESPONDER:
4808 l2cap_do_move_respond(chan, result);
4809 break;
4810 default:
4811 l2cap_do_move_cancel(chan, result);
4812 break;
4813 }
4814 }
8eb200bd
MM		 4815}
4816
8d5a04a1 4817static inline int l2cap_move_channel_req(struct l2cap_conn *conn,
ad0ac6ca
AE		 4818 struct l2cap_cmd_hdr *cmd,
4819 u16 cmd_len, void *data)
8d5a04a1
MM		 4820{
4821 struct l2cap_move_chan_req *req = data;
1500109b 4822 struct l2cap_move_chan_rsp rsp;
02b0fbb9 4823 struct l2cap_chan *chan;
8d5a04a1
MM		 4824 u16 icid = 0;
4825 u16 result = L2CAP_MR_NOT_ALLOWED;
4826
4827 if (cmd_len != sizeof(*req))
4828 return -EPROTO;
4829
4830 icid = le16_to_cpu(req->icid);
4831
ad0ac6ca 4832 BT_DBG("icid 0x%4.4x, dest_amp_id %d", icid, req->dest_amp_id);
8d5a04a1
MM		 4833
4834 if (!enable_hs)
4835 return -EINVAL;
4836
02b0fbb9
MM		 4837 chan = l2cap_get_chan_by_dcid(conn, icid);
4838 if (!chan) {
1500109b
MM		 4839 rsp.icid = cpu_to_le16(icid);
4840 rsp.result = __constant_cpu_to_le16(L2CAP_MR_NOT_ALLOWED);
4841 l2cap_send_cmd(conn, cmd->ident, L2CAP_MOVE_CHAN_RSP,
4842 sizeof(rsp), &rsp);
02b0fbb9
MM		 4843 return 0;
4844 }
4845
1500109b
MM		 4846 chan->ident = cmd->ident;
4847
02b0fbb9
MM		 4848 if (chan->scid < L2CAP_CID_DYN_START ||
4849 chan->chan_policy == BT_CHANNEL_POLICY_BREDR_ONLY ||
4850 (chan->mode != L2CAP_MODE_ERTM &&
4851 chan->mode != L2CAP_MODE_STREAMING)) {
4852 result = L2CAP_MR_NOT_ALLOWED;
4853 goto send_move_response;
4854 }
4855
4856 if (chan->local_amp_id == req->dest_amp_id) {
4857 result = L2CAP_MR_SAME_ID;
4858 goto send_move_response;
4859 }
4860
4861 if (req->dest_amp_id) {
4862 struct hci_dev *hdev;
4863 hdev = hci_dev_get(req->dest_amp_id);
4864 if (!hdev || hdev->dev_type != HCI_AMP ||
4865 !test_bit(HCI_UP, &hdev->flags)) {
4866 if (hdev)
4867 hci_dev_put(hdev);
4868
4869 result = L2CAP_MR_BAD_ID;
4870 goto send_move_response;
4871 }
4872 hci_dev_put(hdev);
4873 }
4874
4875 /* Detect a move collision. Only send a collision response
4876 * if this side has "lost", otherwise proceed with the move.
4877 * The winner has the larger bd_addr.
4878 */
4879 if ((__chan_is_moving(chan) ||
4880 chan->move_role != L2CAP_MOVE_ROLE_NONE) &&
4881 bacmp(conn->src, conn->dst) > 0) {
4882 result = L2CAP_MR_COLLISION;
4883 goto send_move_response;
4884 }
4885
02b0fbb9
MM		 4886 chan->move_role = L2CAP_MOVE_ROLE_RESPONDER;
4887 l2cap_move_setup(chan);
4888 chan->move_id = req->dest_amp_id;
4889 icid = chan->dcid;
4890
4891 if (!req->dest_amp_id) {
4892 /* Moving to BR/EDR */
4893 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
4894 chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
4895 result = L2CAP_MR_PEND;
4896 } else {
4897 chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
4898 result = L2CAP_MR_SUCCESS;
4899 }
4900 } else {
4901 chan->move_state = L2CAP_MOVE_WAIT_PREPARE;
4902 /* Placeholder - uncomment when amp functions are available */
4903 /*amp_accept_physical(chan, req->dest_amp_id);*/
4904 result = L2CAP_MR_PEND;
4905 }
4906
4907send_move_response:
1500109b 4908 l2cap_send_move_chan_rsp(chan, result);
8d5a04a1 4909
02b0fbb9
MM		 4910 l2cap_chan_unlock(chan);
4911
8d5a04a1
MM		 4912 return 0;
4913}
4914
5b155ef9
MM		 4915static void l2cap_move_continue(struct l2cap_conn *conn, u16 icid, u16 result)
4916{
4917 struct l2cap_chan *chan;
4918 struct hci_chan *hchan = NULL;
4919
4920 chan = l2cap_get_chan_by_scid(conn, icid);
4921 if (!chan) {
4922 l2cap_send_move_chan_cfm_icid(conn, icid);
4923 return;
4924 }
4925
4926 __clear_chan_timer(chan);
4927 if (result == L2CAP_MR_PEND)
4928 __set_chan_timer(chan, L2CAP_MOVE_ERTX_TIMEOUT);
4929
4930 switch (chan->move_state) {
4931 case L2CAP_MOVE_WAIT_LOGICAL_COMP:
4932 /* Move confirm will be sent when logical link
4933 * is complete.
4934 */
4935 chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
4936 break;
4937 case L2CAP_MOVE_WAIT_RSP_SUCCESS:
4938 if (result == L2CAP_MR_PEND) {
4939 break;
4940 } else if (test_bit(CONN_LOCAL_BUSY,
4941 &chan->conn_state)) {
4942 chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
4943 } else {
4944 /* Logical link is up or moving to BR/EDR,
4945 * proceed with move
4946 */
4947 chan->move_state = L2CAP_MOVE_WAIT_CONFIRM_RSP;
4948 l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
4949 }
4950 break;
4951 case L2CAP_MOVE_WAIT_RSP:
4952 /* Moving to AMP */
4953 if (result == L2CAP_MR_SUCCESS) {
4954 /* Remote is ready, send confirm immediately
4955 * after logical link is ready
4956 */
4957 chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
4958 } else {
4959 /* Both logical link and move success
4960 * are required to confirm
4961 */
4962 chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_COMP;
4963 }
4964
4965 /* Placeholder - get hci_chan for logical link */
4966 if (!hchan) {
4967 /* Logical link not available */
4968 l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
4969 break;
4970 }
4971
4972 /* If the logical link is not yet connected, do not
4973 * send confirmation.
4974 */
4975 if (hchan->state != BT_CONNECTED)
4976 break;
4977
4978 /* Logical link is already ready to go */
4979
4980 chan->hs_hcon = hchan->conn;
4981 chan->hs_hcon->l2cap_data = chan->conn;
4982
4983 if (result == L2CAP_MR_SUCCESS) {
4984 /* Can confirm now */
4985 l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
4986 } else {
4987 /* Now only need move success
4988 * to confirm
4989 */
4990 chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
4991 }
4992
4993 l2cap_logical_cfm(chan, hchan, L2CAP_MR_SUCCESS);
4994 break;
4995 default:
4996 /* Any other amp move state means the move failed. */
4997 chan->move_id = chan->local_amp_id;
4998 l2cap_move_done(chan);
4999 l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
5000 }
5001
5002 l2cap_chan_unlock(chan);
5003}
5004
5005static void l2cap_move_fail(struct l2cap_conn *conn, u8 ident, u16 icid,
5006 u16 result)
5007{
5008 struct l2cap_chan *chan;
5009
5010 chan = l2cap_get_chan_by_ident(conn, ident);
5011 if (!chan) {
5012 /* Could not locate channel, icid is best guess */
5013 l2cap_send_move_chan_cfm_icid(conn, icid);
5014 return;
5015 }
5016
5017 __clear_chan_timer(chan);
5018
5019 if (chan->move_role == L2CAP_MOVE_ROLE_INITIATOR) {
5020 if (result == L2CAP_MR_COLLISION) {
5021 chan->move_role = L2CAP_MOVE_ROLE_RESPONDER;
5022 } else {
5023 /* Cleanup - cancel move */
5024 chan->move_id = chan->local_amp_id;
5025 l2cap_move_done(chan);
5026 }
5027 }
5028
5029 l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
5030
5031 l2cap_chan_unlock(chan);
5032}
5033
5034static int l2cap_move_channel_rsp(struct l2cap_conn *conn,
5035 struct l2cap_cmd_hdr *cmd,
5036 u16 cmd_len, void *data)
8d5a04a1
MM		 5037{
5038 struct l2cap_move_chan_rsp *rsp = data;
5039 u16 icid, result;
5040
5041 if (cmd_len != sizeof(*rsp))
5042 return -EPROTO;
5043
5044 icid = le16_to_cpu(rsp->icid);
5045 result = le16_to_cpu(rsp->result);
5046
ad0ac6ca 5047 BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result);
8d5a04a1 5048
5b155ef9
MM		 5049 if (result == L2CAP_MR_SUCCESS || result == L2CAP_MR_PEND)
5050 l2cap_move_continue(conn, icid, result);
5051 else
5052 l2cap_move_fail(conn, cmd->ident, icid, result);
8d5a04a1
MM		 5053
5054 return 0;
5055}
5056
5f3847a4
MM		 5057static int l2cap_move_channel_confirm(struct l2cap_conn *conn,
5058 struct l2cap_cmd_hdr *cmd,
5059 u16 cmd_len, void *data)
8d5a04a1
MM		 5060{
5061 struct l2cap_move_chan_cfm *cfm = data;
5f3847a4 5062 struct l2cap_chan *chan;
8d5a04a1
MM		 5063 u16 icid, result;
5064
5065 if (cmd_len != sizeof(*cfm))
5066 return -EPROTO;
5067
5068 icid = le16_to_cpu(cfm->icid);
5069 result = le16_to_cpu(cfm->result);
5070
ad0ac6ca 5071 BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result);
8d5a04a1 5072
5f3847a4
MM		 5073 chan = l2cap_get_chan_by_dcid(conn, icid);
5074 if (!chan) {
5075 /* Spec requires a response even if the icid was not found */
5076 l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid);
5077 return 0;
5078 }
5079
5080 if (chan->move_state == L2CAP_MOVE_WAIT_CONFIRM) {
5081 if (result == L2CAP_MC_CONFIRMED) {
5082 chan->local_amp_id = chan->move_id;
5083 if (!chan->local_amp_id)
5084 __release_logical_link(chan);
5085 } else {
5086 chan->move_id = chan->local_amp_id;
5087 }
5088
5089 l2cap_move_done(chan);
5090 }
5091
8d5a04a1
MM		 5092 l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid);
5093
5f3847a4
MM		 5094 l2cap_chan_unlock(chan);
5095
8d5a04a1
MM		 5096 return 0;
5097}
5098
5099static inline int l2cap_move_channel_confirm_rsp(struct l2cap_conn *conn,
ad0ac6ca
AE		 5100 struct l2cap_cmd_hdr *cmd,
5101 u16 cmd_len, void *data)
8d5a04a1
MM		 5102{
5103 struct l2cap_move_chan_cfm_rsp *rsp = data;
3fd71a0a 5104 struct l2cap_chan *chan;
8d5a04a1
MM		 5105 u16 icid;
5106
5107 if (cmd_len != sizeof(*rsp))
5108 return -EPROTO;
5109
5110 icid = le16_to_cpu(rsp->icid);
5111
ad0ac6ca 5112 BT_DBG("icid 0x%4.4x", icid);
8d5a04a1 5113
3fd71a0a
MM		 5114 chan = l2cap_get_chan_by_scid(conn, icid);
5115 if (!chan)
5116 return 0;
5117
5118 __clear_chan_timer(chan);
5119
5120 if (chan->move_state == L2CAP_MOVE_WAIT_CONFIRM_RSP) {
5121 chan->local_amp_id = chan->move_id;
5122
5123 if (!chan->local_amp_id && chan->hs_hchan)
5124 __release_logical_link(chan);
5125
5126 l2cap_move_done(chan);
5127 }
5128
5129 l2cap_chan_unlock(chan);
5130
8d5a04a1
MM		 5131 return 0;
5132}
5133
e2174ca4 5134static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency,
2d792818 5135 u16 to_multiplier)
de73115a
CT		 5136{
5137 u16 max_latency;
5138
5139 if (min > max || min < 6 || max > 3200)
5140 return -EINVAL;
5141
5142 if (to_multiplier < 10 || to_multiplier > 3200)
5143 return -EINVAL;
5144
5145 if (max >= to_multiplier * 8)
5146 return -EINVAL;
5147
5148 max_latency = (to_multiplier * 8 / max) - 1;
5149 if (latency > 499 || latency > max_latency)
5150 return -EINVAL;
5151
5152 return 0;
5153}
5154
5155static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
2d792818
GP		 5156 struct l2cap_cmd_hdr *cmd,
5157 u8 *data)
de73115a
CT		 5158{
5159 struct hci_conn *hcon = conn->hcon;
5160 struct l2cap_conn_param_update_req *req;
5161 struct l2cap_conn_param_update_rsp rsp;
5162 u16 min, max, latency, to_multiplier, cmd_len;
2ce603eb 5163 int err;
de73115a
CT		 5164
5165 if (!(hcon->link_mode & HCI_LM_MASTER))
5166 return -EINVAL;
5167
5168 cmd_len = __le16_to_cpu(cmd->len);
5169 if (cmd_len != sizeof(struct l2cap_conn_param_update_req))
5170 return -EPROTO;
5171
5172 req = (struct l2cap_conn_param_update_req *) data;
e2174ca4
GP		 5173 min = __le16_to_cpu(req->min);
5174 max = __le16_to_cpu(req->max);
de73115a
CT		 5175 latency = __le16_to_cpu(req->latency);
5176 to_multiplier = __le16_to_cpu(req->to_multiplier);
5177
5178 BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x",
2d792818 5179 min, max, latency, to_multiplier);
de73115a
CT		 5180
5181 memset(&rsp, 0, sizeof(rsp));
2ce603eb
CT		 5182
5183 err = l2cap_check_conn_param(min, max, latency, to_multiplier);
5184 if (err)
ac73498c 5185 rsp.result = __constant_cpu_to_le16(L2CAP_CONN_PARAM_REJECTED);
de73115a 5186 else
ac73498c 5187 rsp.result = __constant_cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED);
de73115a
CT		 5188
5189 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP,
2d792818 5190 sizeof(rsp), &rsp);
de73115a 5191
2ce603eb
CT		 5192 if (!err)
5193 hci_le_conn_update(hcon, min, max, latency, to_multiplier);
5194
de73115a
CT		 5195 return 0;
5196}
5197
3300d9a9 5198static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
2d792818
GP		 5199 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5200 u8 *data)
3300d9a9
CT		 5201{
5202 int err = 0;
5203
5204 switch (cmd->code) {
5205 case L2CAP_COMMAND_REJ:
cb3b3152 5206 l2cap_command_rej(conn, cmd, cmd_len, data);
3300d9a9
CT		 5207 break;
5208
5209 case L2CAP_CONN_REQ:
cb3b3152 5210 err = l2cap_connect_req(conn, cmd, cmd_len, data);
3300d9a9
CT		 5211 break;
5212
5213 case L2CAP_CONN_RSP:
f5a2598d 5214 case L2CAP_CREATE_CHAN_RSP:
cb3b3152 5215 err = l2cap_connect_create_rsp(conn, cmd, cmd_len, data);
3300d9a9
CT		 5216 break;
5217
5218 case L2CAP_CONF_REQ:
5219 err = l2cap_config_req(conn, cmd, cmd_len, data);
5220 break;
5221
5222 case L2CAP_CONF_RSP:
cb3b3152 5223 err = l2cap_config_rsp(conn, cmd, cmd_len, data);
3300d9a9
CT		 5224 break;
5225
5226 case L2CAP_DISCONN_REQ:
cb3b3152 5227 err = l2cap_disconnect_req(conn, cmd, cmd_len, data);
3300d9a9
CT		 5228 break;
5229
5230 case L2CAP_DISCONN_RSP:
cb3b3152 5231 err = l2cap_disconnect_rsp(conn, cmd, cmd_len, data);
3300d9a9
CT		 5232 break;
5233
5234 case L2CAP_ECHO_REQ:
5235 l2cap_send_cmd(conn, cmd->ident, L2CAP_ECHO_RSP, cmd_len, data);
5236 break;
5237
5238 case L2CAP_ECHO_RSP:
5239 break;
5240
5241 case L2CAP_INFO_REQ:
cb3b3152 5242 err = l2cap_information_req(conn, cmd, cmd_len, data);
3300d9a9
CT		 5243 break;
5244
5245 case L2CAP_INFO_RSP:
cb3b3152 5246 err = l2cap_information_rsp(conn, cmd, cmd_len, data);
3300d9a9
CT		 5247 break;
5248
f94ff6ff
MM		 5249 case L2CAP_CREATE_CHAN_REQ:
5250 err = l2cap_create_channel_req(conn, cmd, cmd_len, data);
5251 break;
5252
8d5a04a1
MM		 5253 case L2CAP_MOVE_CHAN_REQ:
5254 err = l2cap_move_channel_req(conn, cmd, cmd_len, data);
5255 break;
5256
5257 case L2CAP_MOVE_CHAN_RSP:
5258 err = l2cap_move_channel_rsp(conn, cmd, cmd_len, data);
5259 break;
5260
5261 case L2CAP_MOVE_CHAN_CFM:
5262 err = l2cap_move_channel_confirm(conn, cmd, cmd_len, data);
5263 break;
5264
5265 case L2CAP_MOVE_CHAN_CFM_RSP:
5266 err = l2cap_move_channel_confirm_rsp(conn, cmd, cmd_len, data);
5267 break;
5268
3300d9a9
CT		 5269 default:
5270 BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code);
5271 err = -EINVAL;
5272 break;
5273 }
5274
5275 return err;
5276}
5277
5278static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn,
2d792818 5279 struct l2cap_cmd_hdr *cmd, u8 *data)
3300d9a9
CT		 5280{
5281 switch (cmd->code) {
5282 case L2CAP_COMMAND_REJ:
5283 return 0;
5284
5285 case L2CAP_CONN_PARAM_UPDATE_REQ:
de73115a 5286 return l2cap_conn_param_update_req(conn, cmd, data);
3300d9a9
CT		 5287
5288 case L2CAP_CONN_PARAM_UPDATE_RSP:
5289 return 0;
5290
5291 default:
5292 BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code);
5293 return -EINVAL;
5294 }
5295}
5296
7c2005d6
JH		 5297static __le16 l2cap_err_to_reason(int err)
5298{
5299 switch (err) {
5300 case -EBADSLT:
5301 return __constant_cpu_to_le16(L2CAP_REJ_INVALID_CID);
5302 case -EMSGSIZE:
5303 return __constant_cpu_to_le16(L2CAP_REJ_MTU_EXCEEDED);
5304 case -EINVAL:
5305 case -EPROTO:
5306 default:
5307 return __constant_cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
5308 }
5309}
5310
c5623556
JH		 5311static inline void l2cap_le_sig_channel(struct l2cap_conn *conn,
5312 struct sk_buff *skb)
5313{
5314 u8 *data = skb->data;
5315 int len = skb->len;
5316 struct l2cap_cmd_hdr cmd;
5317 int err;
5318
5319 l2cap_raw_recv(conn, skb);
5320
5321 while (len >= L2CAP_CMD_HDR_SIZE) {
5322 u16 cmd_len;
5323 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
5324 data += L2CAP_CMD_HDR_SIZE;
5325 len -= L2CAP_CMD_HDR_SIZE;
5326
5327 cmd_len = le16_to_cpu(cmd.len);
5328
5329 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len,
5330 cmd.ident);
5331
5332 if (cmd_len > len || !cmd.ident) {
5333 BT_DBG("corrupted command");
5334 break;
5335 }
5336
5337 err = l2cap_le_sig_cmd(conn, &cmd, data);
5338 if (err) {
5339 struct l2cap_cmd_rej_unk rej;
5340
5341 BT_ERR("Wrong link type (%d)", err);
5342
7c2005d6 5343 rej.reason = l2cap_err_to_reason(err);
c5623556
JH		 5344 l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ,
5345 sizeof(rej), &rej);
5346 }
5347
5348 data += cmd_len;
5349 len -= cmd_len;
5350 }
5351
5352 kfree_skb(skb);
5353}
5354
3300d9a9 5355static inline void l2cap_sig_channel(struct l2cap_conn *conn,
2d792818 5356 struct sk_buff *skb)
1da177e4
LT		 5357{
5358 u8 *data = skb->data;
5359 int len = skb->len;
5360 struct l2cap_cmd_hdr cmd;
3300d9a9 5361 int err;
1da177e4
LT		 5362
5363 l2cap_raw_recv(conn, skb);
5364
5365 while (len >= L2CAP_CMD_HDR_SIZE) {
88219a0f 5366 u16 cmd_len;
1da177e4
LT		 5367 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
5368 data += L2CAP_CMD_HDR_SIZE;
5369 len -= L2CAP_CMD_HDR_SIZE;
5370
88219a0f 5371 cmd_len = le16_to_cpu(cmd.len);
1da177e4 5372
2d792818
GP		 5373 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len,
5374 cmd.ident);
1da177e4 5375
88219a0f 5376 if (cmd_len > len || !cmd.ident) {
1da177e4
LT		 5377 BT_DBG("corrupted command");
5378 break;
5379 }
5380
c5623556 5381 err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data);
1da177e4 5382 if (err) {
e2fd318e 5383 struct l2cap_cmd_rej_unk rej;
2c6d1a2e
GP		 5384
5385 BT_ERR("Wrong link type (%d)", err);
1da177e4 5386
7c2005d6 5387 rej.reason = l2cap_err_to_reason(err);
2d792818
GP		 5388 l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ,
5389 sizeof(rej), &rej);
1da177e4
LT		 5390 }
5391
88219a0f
AV		 5392 data += cmd_len;
5393 len -= cmd_len;
1da177e4
LT		 5394 }
5395
5396 kfree_skb(skb);
5397}
5398
47d1ec61 5399static int l2cap_check_fcs(struct l2cap_chan *chan, struct sk_buff *skb)
fcc203c3
GP		 5400{
5401 u16 our_fcs, rcv_fcs;
e4ca6d98
AE		 5402 int hdr_size;
5403
5404 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
5405 hdr_size = L2CAP_EXT_HDR_SIZE;
5406 else
5407 hdr_size = L2CAP_ENH_HDR_SIZE;
fcc203c3 5408
47d1ec61 5409 if (chan->fcs == L2CAP_FCS_CRC16) {
03a51213 5410 skb_trim(skb, skb->len - L2CAP_FCS_SIZE);
fcc203c3
GP		 5411 rcv_fcs = get_unaligned_le16(skb->data + skb->len);
5412 our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
5413
5414 if (our_fcs != rcv_fcs)
7a560e5c 5415 return -EBADMSG;
fcc203c3
GP		 5416 }
5417 return 0;
5418}
5419
6ea00485 5420static void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan)
d5392c8f 5421{
e31f7633 5422 struct l2cap_ctrl control;
d5392c8f 5423
e31f7633 5424 BT_DBG("chan %p", chan);
d5392c8f 5425
e31f7633
MM		 5426 memset(&control, 0, sizeof(control));
5427 control.sframe = 1;
5428 control.final = 1;
5429 control.reqseq = chan->buffer_seq;
5430 set_bit(CONN_SEND_FBIT, &chan->conn_state);
d5392c8f 5431
e2ab4353 5432 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
e31f7633
MM		 5433 control.super = L2CAP_SUPER_RNR;
5434 l2cap_send_sframe(chan, &control);
d5392c8f
GP		 5435 }
5436
e31f7633
MM		 5437 if (test_and_clear_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
5438 chan->unacked_frames > 0)
5439 __set_retrans_timer(chan);
d5392c8f 5440
e31f7633 5441 /* Send pending iframes */
525cd185 5442 l2cap_ertm_send(chan);
d5392c8f 5443
e2ab4353 5444 if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state) &&
e31f7633
MM		 5445 test_bit(CONN_SEND_FBIT, &chan->conn_state)) {
5446 /* F-bit wasn't sent in an s-frame or i-frame yet, so
5447 * send it now.
5448 */
5449 control.super = L2CAP_SUPER_RR;
5450 l2cap_send_sframe(chan, &control);
d5392c8f
GP		 5451 }
5452}
5453
2d792818
GP		 5454static void append_skb_frag(struct sk_buff *skb, struct sk_buff *new_frag,
5455 struct sk_buff **last_frag)
18778a63 5456{
84084a31
MM		 5457 /* skb->len reflects data in skb as well as all fragments
5458 * skb->data_len reflects only data in fragments
5459 */
5460 if (!skb_has_frag_list(skb))
5461 skb_shinfo(skb)->frag_list = new_frag;
5462
5463 new_frag->next = NULL;
5464
5465 (*last_frag)->next = new_frag;
5466 *last_frag = new_frag;
5467
5468 skb->len += new_frag->len;
5469 skb->data_len += new_frag->len;
5470 skb->truesize += new_frag->truesize;
5471}
5472
4b51dae9
MM		 5473static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb,
5474 struct l2cap_ctrl *control)
84084a31
MM		 5475{
5476 int err = -EINVAL;
18778a63 5477
4b51dae9 5478 switch (control->sar) {
7e0ef6ee 5479 case L2CAP_SAR_UNSEGMENTED:
84084a31
MM		 5480 if (chan->sdu)
5481 break;
18778a63 5482
80b98027 5483 err = chan->ops->recv(chan, skb);
84084a31 5484 break;
18778a63 5485
7e0ef6ee 5486 case L2CAP_SAR_START:
84084a31
MM		 5487 if (chan->sdu)
5488 break;
18778a63 5489
6f61fd47 5490 chan->sdu_len = get_unaligned_le16(skb->data);
03a51213 5491 skb_pull(skb, L2CAP_SDULEN_SIZE);
18778a63 5492
84084a31
MM		 5493 if (chan->sdu_len > chan->imtu) {
5494 err = -EMSGSIZE;
5495 break;
5496 }
1890d36b 5497
84084a31
MM		 5498 if (skb->len >= chan->sdu_len)
5499 break;
18778a63 5500
84084a31
MM		 5501 chan->sdu = skb;
5502 chan->sdu_last_frag = skb;
18778a63 5503
84084a31
MM		 5504 skb = NULL;
5505 err = 0;
18778a63
GP		 5506 break;
5507
7e0ef6ee 5508 case L2CAP_SAR_CONTINUE:
6f61fd47 5509 if (!chan->sdu)
84084a31 5510 break;
18778a63 5511
84084a31
MM		 5512 append_skb_frag(chan->sdu, skb,
5513 &chan->sdu_last_frag);
5514 skb = NULL;
18778a63 5515
84084a31
MM		 5516 if (chan->sdu->len >= chan->sdu_len)
5517 break;
4178ba46 5518
84084a31 5519 err = 0;
18778a63
GP		 5520 break;
5521
7e0ef6ee 5522 case L2CAP_SAR_END:
6f61fd47 5523 if (!chan->sdu)
84084a31 5524 break;
18778a63 5525
84084a31
MM		 5526 append_skb_frag(chan->sdu, skb,
5527 &chan->sdu_last_frag);
5528 skb = NULL;
4178ba46 5529
84084a31
MM		 5530 if (chan->sdu->len != chan->sdu_len)
5531 break;
18778a63 5532
80b98027 5533 err = chan->ops->recv(chan, chan->sdu);
1890d36b 5534
84084a31
MM		 5535 if (!err) {
5536 /* Reassembly complete */
5537 chan->sdu = NULL;
5538 chan->sdu_last_frag = NULL;
5539 chan->sdu_len = 0;
1890d36b 5540 }
18778a63
GP		 5541 break;
5542 }
5543
84084a31
MM		 5544 if (err) {
5545 kfree_skb(skb);
5546 kfree_skb(chan->sdu);
5547 chan->sdu = NULL;
5548 chan->sdu_last_frag = NULL;
5549 chan->sdu_len = 0;
5550 }
18778a63 5551
84084a31 5552 return err;
18778a63
GP		 5553}
5554
32b32735
MM		 5555static int l2cap_resegment(struct l2cap_chan *chan)
5556{
5557 /* Placeholder */
5558 return 0;
5559}
5560
61aa4f5b 5561void l2cap_chan_busy(struct l2cap_chan *chan, int busy)
26f880d2 5562{
61aa4f5b 5563 u8 event;
712132eb 5564
61aa4f5b
MM		 5565 if (chan->mode != L2CAP_MODE_ERTM)
5566 return;
712132eb 5567
61aa4f5b 5568 event = busy ? L2CAP_EV_LOCAL_BUSY_DETECTED : L2CAP_EV_LOCAL_BUSY_CLEAR;
401bb1f7 5569 l2cap_tx(chan, NULL, NULL, event);
1890d36b
GP		 5570}
5571
d2a7ac5d
MM		 5572static int l2cap_rx_queued_iframes(struct l2cap_chan *chan)
5573{
63838725
MM		 5574 int err = 0;
5575 /* Pass sequential frames to l2cap_reassemble_sdu()
5576 * until a gap is encountered.
5577 */
5578
5579 BT_DBG("chan %p", chan);
5580
5581 while (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
5582 struct sk_buff *skb;
5583 BT_DBG("Searching for skb with txseq %d (queue len %d)",
5584 chan->buffer_seq, skb_queue_len(&chan->srej_q));
5585
5586 skb = l2cap_ertm_seq_in_queue(&chan->srej_q, chan->buffer_seq);
5587
5588 if (!skb)
5589 break;
5590
5591 skb_unlink(skb, &chan->srej_q);
5592 chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
5593 err = l2cap_reassemble_sdu(chan, skb, &bt_cb(skb)->control);
5594 if (err)
5595 break;
5596 }
5597
5598 if (skb_queue_empty(&chan->srej_q)) {
5599 chan->rx_state = L2CAP_RX_STATE_RECV;
5600 l2cap_send_ack(chan);
5601 }
5602
5603 return err;
d2a7ac5d
MM		 5604}
5605
5606static void l2cap_handle_srej(struct l2cap_chan *chan,
5607 struct l2cap_ctrl *control)
5608{
f80842a8
MM		 5609 struct sk_buff *skb;
5610
5611 BT_DBG("chan %p, control %p", chan, control);
5612
5613 if (control->reqseq == chan->next_tx_seq) {
5614 BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq);
5e4e3972 5615 l2cap_send_disconn_req(chan, ECONNRESET);
f80842a8
MM		 5616 return;
5617 }
5618
5619 skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq);
5620
5621 if (skb == NULL) {
5622 BT_DBG("Seq %d not available for retransmission",
5623 control->reqseq);
5624 return;
5625 }
5626
5627 if (chan->max_tx != 0 && bt_cb(skb)->control.retries >= chan->max_tx) {
5628 BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
5e4e3972 5629 l2cap_send_disconn_req(chan, ECONNRESET);
f80842a8
MM		 5630 return;
5631 }
5632
5633 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
5634
5635 if (control->poll) {
5636 l2cap_pass_to_tx(chan, control);
5637
5638 set_bit(CONN_SEND_FBIT, &chan->conn_state);
5639 l2cap_retransmit(chan, control);
5640 l2cap_ertm_send(chan);
5641
5642 if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) {
5643 set_bit(CONN_SREJ_ACT, &chan->conn_state);
5644 chan->srej_save_reqseq = control->reqseq;
5645 }
5646 } else {
5647 l2cap_pass_to_tx_fbit(chan, control);
5648
5649 if (control->final) {
5650 if (chan->srej_save_reqseq != control->reqseq ||
5651 !test_and_clear_bit(CONN_SREJ_ACT,
5652 &chan->conn_state))
5653 l2cap_retransmit(chan, control);
5654 } else {
5655 l2cap_retransmit(chan, control);
5656 if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) {
5657 set_bit(CONN_SREJ_ACT, &chan->conn_state);
5658 chan->srej_save_reqseq = control->reqseq;
5659 }
5660 }
5661 }
d2a7ac5d
MM		 5662}
5663
5664static void l2cap_handle_rej(struct l2cap_chan *chan,
5665 struct l2cap_ctrl *control)
5666{
fcd289df
MM		 5667 struct sk_buff *skb;
5668
5669 BT_DBG("chan %p, control %p", chan, control);
5670
5671 if (control->reqseq == chan->next_tx_seq) {
5672 BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq);
5e4e3972 5673 l2cap_send_disconn_req(chan, ECONNRESET);
fcd289df
MM		 5674 return;
5675 }
5676
5677 skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq);
5678
5679 if (chan->max_tx && skb &&
5680 bt_cb(skb)->control.retries >= chan->max_tx) {
5681 BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
5e4e3972 5682 l2cap_send_disconn_req(chan, ECONNRESET);
fcd289df
MM		 5683 return;
5684 }
5685
5686 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
5687
5688 l2cap_pass_to_tx(chan, control);
5689
5690 if (control->final) {
5691 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
5692 l2cap_retransmit_all(chan, control);
5693 } else {
5694 l2cap_retransmit_all(chan, control);
5695 l2cap_ertm_send(chan);
5696 if (chan->tx_state == L2CAP_TX_STATE_WAIT_F)
5697 set_bit(CONN_REJ_ACT, &chan->conn_state);
5698 }
d2a7ac5d
MM		 5699}
5700
4b51dae9
MM		 5701static u8 l2cap_classify_txseq(struct l2cap_chan *chan, u16 txseq)
5702{
5703 BT_DBG("chan %p, txseq %d", chan, txseq);
5704
5705 BT_DBG("last_acked_seq %d, expected_tx_seq %d", chan->last_acked_seq,
5706 chan->expected_tx_seq);
5707
5708 if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
5709 if (__seq_offset(chan, txseq, chan->last_acked_seq) >=
2d792818 5710 chan->tx_win) {
4b51dae9
MM		 5711 /* See notes below regarding "double poll" and
5712 * invalid packets.
5713 */
5714 if (chan->tx_win <= ((chan->tx_win_max + 1) >> 1)) {
5715 BT_DBG("Invalid/Ignore - after SREJ");
5716 return L2CAP_TXSEQ_INVALID_IGNORE;
5717 } else {
5718 BT_DBG("Invalid - in window after SREJ sent");
5719 return L2CAP_TXSEQ_INVALID;
5720 }
5721 }
5722
5723 if (chan->srej_list.head == txseq) {
5724 BT_DBG("Expected SREJ");
5725 return L2CAP_TXSEQ_EXPECTED_SREJ;
5726 }
5727
5728 if (l2cap_ertm_seq_in_queue(&chan->srej_q, txseq)) {
5729 BT_DBG("Duplicate SREJ - txseq already stored");
5730 return L2CAP_TXSEQ_DUPLICATE_SREJ;
5731 }
5732
5733 if (l2cap_seq_list_contains(&chan->srej_list, txseq)) {
5734 BT_DBG("Unexpected SREJ - not requested");
5735 return L2CAP_TXSEQ_UNEXPECTED_SREJ;
5736 }
5737 }
5738
5739 if (chan->expected_tx_seq == txseq) {
5740 if (__seq_offset(chan, txseq, chan->last_acked_seq) >=
5741 chan->tx_win) {
5742 BT_DBG("Invalid - txseq outside tx window");
5743 return L2CAP_TXSEQ_INVALID;
5744 } else {
5745 BT_DBG("Expected");
5746 return L2CAP_TXSEQ_EXPECTED;
5747 }
5748 }
5749
5750 if (__seq_offset(chan, txseq, chan->last_acked_seq) <
2d792818 5751 __seq_offset(chan, chan->expected_tx_seq, chan->last_acked_seq)) {
4b51dae9
MM		 5752 BT_DBG("Duplicate - expected_tx_seq later than txseq");
5753 return L2CAP_TXSEQ_DUPLICATE;
5754 }
5755
5756 if (__seq_offset(chan, txseq, chan->last_acked_seq) >= chan->tx_win) {
5757 /* A source of invalid packets is a "double poll" condition,
5758 * where delays cause us to send multiple poll packets. If
5759 * the remote stack receives and processes both polls,
5760 * sequence numbers can wrap around in such a way that a
5761 * resent frame has a sequence number that looks like new data
5762 * with a sequence gap. This would trigger an erroneous SREJ
5763 * request.
5764 *
5765 * Fortunately, this is impossible with a tx window that's
5766 * less than half of the maximum sequence number, which allows
5767 * invalid frames to be safely ignored.
5768 *
5769 * With tx window sizes greater than half of the tx window
5770 * maximum, the frame is invalid and cannot be ignored. This
5771 * causes a disconnect.
5772 */
5773
5774 if (chan->tx_win <= ((chan->tx_win_max + 1) >> 1)) {
5775 BT_DBG("Invalid/Ignore - txseq outside tx window");
5776 return L2CAP_TXSEQ_INVALID_IGNORE;
5777 } else {
5778 BT_DBG("Invalid - txseq outside tx window");
5779 return L2CAP_TXSEQ_INVALID;
5780 }
5781 } else {
5782 BT_DBG("Unexpected - txseq indicates missing frames");
5783 return L2CAP_TXSEQ_UNEXPECTED;
5784 }
5785}
5786
d2a7ac5d
MM		 5787static int l2cap_rx_state_recv(struct l2cap_chan *chan,
5788 struct l2cap_ctrl *control,
5789 struct sk_buff *skb, u8 event)
5790{
5791 int err = 0;
5792 bool skb_in_use = 0;
5793
5794 BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
5795 event);
5796
5797 switch (event) {
5798 case L2CAP_EV_RECV_IFRAME:
5799 switch (l2cap_classify_txseq(chan, control->txseq)) {
5800 case L2CAP_TXSEQ_EXPECTED:
5801 l2cap_pass_to_tx(chan, control);
5802
5803 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
5804 BT_DBG("Busy, discarding expected seq %d",
5805 control->txseq);
5806 break;
5807 }
5808
5809 chan->expected_tx_seq = __next_seq(chan,
5810 control->txseq);
5811
5812 chan->buffer_seq = chan->expected_tx_seq;
5813 skb_in_use = 1;
5814
5815 err = l2cap_reassemble_sdu(chan, skb, control);
5816 if (err)
5817 break;
5818
5819 if (control->final) {
5820 if (!test_and_clear_bit(CONN_REJ_ACT,
5821 &chan->conn_state)) {
5822 control->final = 0;
5823 l2cap_retransmit_all(chan, control);
5824 l2cap_ertm_send(chan);
5825 }
5826 }
5827
5828 if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state))
5829 l2cap_send_ack(chan);
5830 break;
5831 case L2CAP_TXSEQ_UNEXPECTED:
5832 l2cap_pass_to_tx(chan, control);
5833
5834 /* Can't issue SREJ frames in the local busy state.
5835 * Drop this frame, it will be seen as missing
5836 * when local busy is exited.
5837 */
5838 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
5839 BT_DBG("Busy, discarding unexpected seq %d",
5840 control->txseq);
5841 break;
5842 }
5843
5844 /* There was a gap in the sequence, so an SREJ
5845 * must be sent for each missing frame. The
5846 * current frame is stored for later use.
5847 */
5848 skb_queue_tail(&chan->srej_q, skb);
5849 skb_in_use = 1;
5850 BT_DBG("Queued %p (queue len %d)", skb,
5851 skb_queue_len(&chan->srej_q));
5852
5853 clear_bit(CONN_SREJ_ACT, &chan->conn_state);
5854 l2cap_seq_list_clear(&chan->srej_list);
5855 l2cap_send_srej(chan, control->txseq);
5856
5857 chan->rx_state = L2CAP_RX_STATE_SREJ_SENT;
5858 break;
5859 case L2CAP_TXSEQ_DUPLICATE:
5860 l2cap_pass_to_tx(chan, control);
5861 break;
5862 case L2CAP_TXSEQ_INVALID_IGNORE:
5863 break;
5864 case L2CAP_TXSEQ_INVALID:
5865 default:
5e4e3972 5866 l2cap_send_disconn_req(chan, ECONNRESET);
d2a7ac5d
MM		 5867 break;
5868 }
5869 break;
5870 case L2CAP_EV_RECV_RR:
5871 l2cap_pass_to_tx(chan, control);
5872 if (control->final) {
5873 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
5874
e6a3ee6e
MM		 5875 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state) &&
5876 !__chan_is_moving(chan)) {
d2a7ac5d
MM		 5877 control->final = 0;
5878 l2cap_retransmit_all(chan, control);
5879 }
5880
5881 l2cap_ertm_send(chan);
5882 } else if (control->poll) {
5883 l2cap_send_i_or_rr_or_rnr(chan);
5884 } else {
5885 if (test_and_clear_bit(CONN_REMOTE_BUSY,
5886 &chan->conn_state) &&
5887 chan->unacked_frames)
5888 __set_retrans_timer(chan);
5889
5890 l2cap_ertm_send(chan);
5891 }
5892 break;
5893 case L2CAP_EV_RECV_RNR:
5894 set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
5895 l2cap_pass_to_tx(chan, control);
5896 if (control && control->poll) {
5897 set_bit(CONN_SEND_FBIT, &chan->conn_state);
5898 l2cap_send_rr_or_rnr(chan, 0);
5899 }
5900 __clear_retrans_timer(chan);
5901 l2cap_seq_list_clear(&chan->retrans_list);
5902 break;
5903 case L2CAP_EV_RECV_REJ:
5904 l2cap_handle_rej(chan, control);
5905 break;
5906 case L2CAP_EV_RECV_SREJ:
5907 l2cap_handle_srej(chan, control);
5908 break;
5909 default:
5910 break;
5911 }
5912
5913 if (skb && !skb_in_use) {
5914 BT_DBG("Freeing %p", skb);
5915 kfree_skb(skb);
5916 }
5917
5918 return err;
5919}
5920
5921static int l2cap_rx_state_srej_sent(struct l2cap_chan *chan,
5922 struct l2cap_ctrl *control,
5923 struct sk_buff *skb, u8 event)
5924{
5925 int err = 0;
5926 u16 txseq = control->txseq;
5927 bool skb_in_use = 0;
5928
5929 BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
5930 event);
5931
5932 switch (event) {
5933 case L2CAP_EV_RECV_IFRAME:
5934 switch (l2cap_classify_txseq(chan, txseq)) {
5935 case L2CAP_TXSEQ_EXPECTED:
5936 /* Keep frame for reassembly later */
5937 l2cap_pass_to_tx(chan, control);
5938 skb_queue_tail(&chan->srej_q, skb);
5939 skb_in_use = 1;
5940 BT_DBG("Queued %p (queue len %d)", skb,
5941 skb_queue_len(&chan->srej_q));
5942
5943 chan->expected_tx_seq = __next_seq(chan, txseq);
5944 break;
5945 case L2CAP_TXSEQ_EXPECTED_SREJ:
5946 l2cap_seq_list_pop(&chan->srej_list);
5947
5948 l2cap_pass_to_tx(chan, control);
5949 skb_queue_tail(&chan->srej_q, skb);
5950 skb_in_use = 1;
5951 BT_DBG("Queued %p (queue len %d)", skb,
5952 skb_queue_len(&chan->srej_q));
5953
5954 err = l2cap_rx_queued_iframes(chan);
5955 if (err)
5956 break;
5957
5958 break;
5959 case L2CAP_TXSEQ_UNEXPECTED:
5960 /* Got a frame that can't be reassembled yet.
5961 * Save it for later, and send SREJs to cover
5962 * the missing frames.
5963 */
5964 skb_queue_tail(&chan->srej_q, skb);
5965 skb_in_use = 1;
5966 BT_DBG("Queued %p (queue len %d)", skb,
5967 skb_queue_len(&chan->srej_q));
5968
5969 l2cap_pass_to_tx(chan, control);
5970 l2cap_send_srej(chan, control->txseq);
5971 break;
5972 case L2CAP_TXSEQ_UNEXPECTED_SREJ:
5973 /* This frame was requested with an SREJ, but
5974 * some expected retransmitted frames are
5975 * missing. Request retransmission of missing
5976 * SREJ'd frames.
5977 */
5978 skb_queue_tail(&chan->srej_q, skb);
5979 skb_in_use = 1;
5980 BT_DBG("Queued %p (queue len %d)", skb,
5981 skb_queue_len(&chan->srej_q));
5982
5983 l2cap_pass_to_tx(chan, control);
5984 l2cap_send_srej_list(chan, control->txseq);
5985 break;
5986 case L2CAP_TXSEQ_DUPLICATE_SREJ:
5987 /* We've already queued this frame. Drop this copy. */
5988 l2cap_pass_to_tx(chan, control);
5989 break;
5990 case L2CAP_TXSEQ_DUPLICATE:
5991 /* Expecting a later sequence number, so this frame
5992 * was already received. Ignore it completely.
5993 */
5994 break;
5995 case L2CAP_TXSEQ_INVALID_IGNORE:
5996 break;
5997 case L2CAP_TXSEQ_INVALID:
5998 default:
5e4e3972 5999 l2cap_send_disconn_req(chan, ECONNRESET);
d2a7ac5d
MM		 6000 break;
6001 }
6002 break;
6003 case L2CAP_EV_RECV_RR:
6004 l2cap_pass_to_tx(chan, control);
6005 if (control->final) {
6006 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6007
6008 if (!test_and_clear_bit(CONN_REJ_ACT,
6009 &chan->conn_state)) {
6010 control->final = 0;
6011 l2cap_retransmit_all(chan, control);
6012 }
6013
6014 l2cap_ertm_send(chan);
6015 } else if (control->poll) {
6016 if (test_and_clear_bit(CONN_REMOTE_BUSY,
6017 &chan->conn_state) &&
6018 chan->unacked_frames) {
6019 __set_retrans_timer(chan);
6020 }
6021
6022 set_bit(CONN_SEND_FBIT, &chan->conn_state);
6023 l2cap_send_srej_tail(chan);
6024 } else {
6025 if (test_and_clear_bit(CONN_REMOTE_BUSY,
6026 &chan->conn_state) &&
6027 chan->unacked_frames)
6028 __set_retrans_timer(chan);
6029
6030 l2cap_send_ack(chan);
6031 }
6032 break;
6033 case L2CAP_EV_RECV_RNR:
6034 set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6035 l2cap_pass_to_tx(chan, control);
6036 if (control->poll) {
6037 l2cap_send_srej_tail(chan);
6038 } else {
6039 struct l2cap_ctrl rr_control;
6040 memset(&rr_control, 0, sizeof(rr_control));
6041 rr_control.sframe = 1;
6042 rr_control.super = L2CAP_SUPER_RR;
6043 rr_control.reqseq = chan->buffer_seq;
6044 l2cap_send_sframe(chan, &rr_control);
6045 }
6046
6047 break;
6048 case L2CAP_EV_RECV_REJ:
6049 l2cap_handle_rej(chan, control);
6050 break;
6051 case L2CAP_EV_RECV_SREJ:
6052 l2cap_handle_srej(chan, control);
6053 break;
6054 }
6055
6056 if (skb && !skb_in_use) {
6057 BT_DBG("Freeing %p", skb);
6058 kfree_skb(skb);
6059 }
6060
6061 return err;
6062}
6063
32b32735
MM		 6064static int l2cap_finish_move(struct l2cap_chan *chan)
6065{
6066 BT_DBG("chan %p", chan);
6067
6068 chan->rx_state = L2CAP_RX_STATE_RECV;
6069
6070 if (chan->hs_hcon)
6071 chan->conn->mtu = chan->hs_hcon->hdev->block_mtu;
6072 else
6073 chan->conn->mtu = chan->conn->hcon->hdev->acl_mtu;
6074
6075 return l2cap_resegment(chan);
6076}
6077
6078static int l2cap_rx_state_wait_p(struct l2cap_chan *chan,
6079 struct l2cap_ctrl *control,
6080 struct sk_buff *skb, u8 event)
6081{
6082 int err;
6083
6084 BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
6085 event);
6086
6087 if (!control->poll)
6088 return -EPROTO;
6089
6090 l2cap_process_reqseq(chan, control->reqseq);
6091
6092 if (!skb_queue_empty(&chan->tx_q))
6093 chan->tx_send_head = skb_peek(&chan->tx_q);
6094 else
6095 chan->tx_send_head = NULL;
6096
6097 /* Rewind next_tx_seq to the point expected
6098 * by the receiver.
6099 */
6100 chan->next_tx_seq = control->reqseq;
6101 chan->unacked_frames = 0;
6102
6103 err = l2cap_finish_move(chan);
6104 if (err)
6105 return err;
6106
6107 set_bit(CONN_SEND_FBIT, &chan->conn_state);
6108 l2cap_send_i_or_rr_or_rnr(chan);
6109
6110 if (event == L2CAP_EV_RECV_IFRAME)
6111 return -EPROTO;
6112
6113 return l2cap_rx_state_recv(chan, control, NULL, event);
6114}
6115
6116static int l2cap_rx_state_wait_f(struct l2cap_chan *chan,
6117 struct l2cap_ctrl *control,
6118 struct sk_buff *skb, u8 event)
6119{
6120 int err;
6121
6122 if (!control->final)
6123 return -EPROTO;
6124
6125 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6126
6127 chan->rx_state = L2CAP_RX_STATE_RECV;
6128 l2cap_process_reqseq(chan, control->reqseq);
6129
6130 if (!skb_queue_empty(&chan->tx_q))
6131 chan->tx_send_head = skb_peek(&chan->tx_q);
6132 else
6133 chan->tx_send_head = NULL;
6134
6135 /* Rewind next_tx_seq to the point expected
6136 * by the receiver.
6137 */
6138 chan->next_tx_seq = control->reqseq;
6139 chan->unacked_frames = 0;
6140
6141 if (chan->hs_hcon)
6142 chan->conn->mtu = chan->hs_hcon->hdev->block_mtu;
6143 else
6144 chan->conn->mtu = chan->conn->hcon->hdev->acl_mtu;
6145
6146 err = l2cap_resegment(chan);
6147
6148 if (!err)
6149 err = l2cap_rx_state_recv(chan, control, skb, event);
6150
6151 return err;
6152}
6153
d2a7ac5d
MM		 6154static bool __valid_reqseq(struct l2cap_chan *chan, u16 reqseq)
6155{
6156 /* Make sure reqseq is for a packet that has been sent but not acked */
6157 u16 unacked;
6158
6159 unacked = __seq_offset(chan, chan->next_tx_seq, chan->expected_ack_seq);
6160 return __seq_offset(chan, chan->next_tx_seq, reqseq) <= unacked;
6161}
6162
cec8ab6e
MM		 6163static int l2cap_rx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
6164 struct sk_buff *skb, u8 event)
218bb9df 6165{
d2a7ac5d
MM		 6166 int err = 0;
6167
6168 BT_DBG("chan %p, control %p, skb %p, event %d, state %d", chan,
6169 control, skb, event, chan->rx_state);
6170
6171 if (__valid_reqseq(chan, control->reqseq)) {
6172 switch (chan->rx_state) {
6173 case L2CAP_RX_STATE_RECV:
6174 err = l2cap_rx_state_recv(chan, control, skb, event);
6175 break;
6176 case L2CAP_RX_STATE_SREJ_SENT:
6177 err = l2cap_rx_state_srej_sent(chan, control, skb,
6178 event);
6179 break;
32b32735
MM		 6180 case L2CAP_RX_STATE_WAIT_P:
6181 err = l2cap_rx_state_wait_p(chan, control, skb, event);
6182 break;
6183 case L2CAP_RX_STATE_WAIT_F:
6184 err = l2cap_rx_state_wait_f(chan, control, skb, event);
6185 break;
d2a7ac5d
MM		 6186 default:
6187 /* shut it down */
6188 break;
6189 }
6190 } else {
6191 BT_DBG("Invalid reqseq %d (next_tx_seq %d, expected_ack_seq %d",
6192 control->reqseq, chan->next_tx_seq,
6193 chan->expected_ack_seq);
5e4e3972 6194 l2cap_send_disconn_req(chan, ECONNRESET);
d2a7ac5d
MM		 6195 }
6196
6197 return err;
cec8ab6e
MM		 6198}
6199
6200static int l2cap_stream_rx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
6201 struct sk_buff *skb)
6202{
4b51dae9
MM		 6203 int err = 0;
6204
6205 BT_DBG("chan %p, control %p, skb %p, state %d", chan, control, skb,
6206 chan->rx_state);
6207
6208 if (l2cap_classify_txseq(chan, control->txseq) ==
6209 L2CAP_TXSEQ_EXPECTED) {
6210 l2cap_pass_to_tx(chan, control);
6211
6212 BT_DBG("buffer_seq %d->%d", chan->buffer_seq,
6213 __next_seq(chan, chan->buffer_seq));
6214
6215 chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
6216
6217 l2cap_reassemble_sdu(chan, skb, control);
6218 } else {
6219 if (chan->sdu) {
6220 kfree_skb(chan->sdu);
6221 chan->sdu = NULL;
6222 }
6223 chan->sdu_last_frag = NULL;
6224 chan->sdu_len = 0;
6225
6226 if (skb) {
6227 BT_DBG("Freeing %p", skb);
6228 kfree_skb(skb);
6229 }
6230 }
6231
6232 chan->last_acked_seq = control->txseq;
6233 chan->expected_tx_seq = __next_seq(chan, control->txseq);
6234
6235 return err;
cec8ab6e
MM		 6236}
6237
6238static int l2cap_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb)
6239{
6240 struct l2cap_ctrl *control = &bt_cb(skb)->control;
6241 u16 len;
6242 u8 event;
218bb9df 6243
b76bbd66
MM		 6244 __unpack_control(chan, skb);
6245
218bb9df
GP		 6246 len = skb->len;
6247
6248 /*
6249 * We can just drop the corrupted I-frame here.
6250 * Receiver will miss it and start proper recovery
cec8ab6e 6251 * procedures and ask for retransmission.
218bb9df 6252 */
47d1ec61 6253 if (l2cap_check_fcs(chan, skb))
218bb9df
GP		 6254 goto drop;
6255
cec8ab6e 6256 if (!control->sframe && control->sar == L2CAP_SAR_START)
03a51213 6257 len -= L2CAP_SDULEN_SIZE;
218bb9df 6258
47d1ec61 6259 if (chan->fcs == L2CAP_FCS_CRC16)
03a51213 6260 len -= L2CAP_FCS_SIZE;
218bb9df 6261
47d1ec61 6262 if (len > chan->mps) {
5e4e3972 6263 l2cap_send_disconn_req(chan, ECONNRESET);
218bb9df
GP		 6264 goto drop;
6265 }
6266
cec8ab6e
MM		 6267 if (!control->sframe) {
6268 int err;
218bb9df 6269
cec8ab6e
MM		 6270 BT_DBG("iframe sar %d, reqseq %d, final %d, txseq %d",
6271 control->sar, control->reqseq, control->final,
6272 control->txseq);
218bb9df 6273
cec8ab6e
MM		 6274 /* Validate F-bit - F=0 always valid, F=1 only
6275 * valid in TX WAIT_F
6276 */
6277 if (control->final && chan->tx_state != L2CAP_TX_STATE_WAIT_F)
218bb9df 6278 goto drop;
cec8ab6e
MM		 6279
6280 if (chan->mode != L2CAP_MODE_STREAMING) {
6281 event = L2CAP_EV_RECV_IFRAME;
6282 err = l2cap_rx(chan, control, skb, event);
6283 } else {
6284 err = l2cap_stream_rx(chan, control, skb);
218bb9df
GP		 6285 }
6286
cec8ab6e 6287 if (err)
5e4e3972 6288 l2cap_send_disconn_req(chan, ECONNRESET);
218bb9df 6289 } else {
cec8ab6e
MM		 6290 const u8 rx_func_to_event[4] = {
6291 L2CAP_EV_RECV_RR, L2CAP_EV_RECV_REJ,
6292 L2CAP_EV_RECV_RNR, L2CAP_EV_RECV_SREJ
6293 };
6294
6295 /* Only I-frames are expected in streaming mode */
6296 if (chan->mode == L2CAP_MODE_STREAMING)
6297 goto drop;
6298
6299 BT_DBG("sframe reqseq %d, final %d, poll %d, super %d",
6300 control->reqseq, control->final, control->poll,
6301 control->super);
6302
218bb9df 6303 if (len != 0) {
1bb166e6 6304 BT_ERR("Trailing bytes: %d in sframe", len);
5e4e3972 6305 l2cap_send_disconn_req(chan, ECONNRESET);
218bb9df
GP		 6306 goto drop;
6307 }
6308
cec8ab6e
MM		 6309 /* Validate F and P bits */
6310 if (control->final && (control->poll ||
6311 chan->tx_state != L2CAP_TX_STATE_WAIT_F))
6312 goto drop;
6313
6314 event = rx_func_to_event[control->super];
6315 if (l2cap_rx(chan, control, skb, event))
5e4e3972 6316 l2cap_send_disconn_req(chan, ECONNRESET);
218bb9df
GP		 6317 }
6318
6319 return 0;
6320
6321drop:
6322 kfree_skb(skb);
6323 return 0;
6324}
6325
13ca56e0
AE		 6326static void l2cap_data_channel(struct l2cap_conn *conn, u16 cid,
6327 struct sk_buff *skb)
1da177e4 6328{
48454079 6329 struct l2cap_chan *chan;
1da177e4 6330
baa7e1fa 6331 chan = l2cap_get_chan_by_scid(conn, cid);
48454079 6332 if (!chan) {
97e8e89d
AE		 6333 if (cid == L2CAP_CID_A2MP) {
6334 chan = a2mp_channel_create(conn, skb);
6335 if (!chan) {
6336 kfree_skb(skb);
13ca56e0 6337 return;
97e8e89d
AE		 6338 }
6339
6340 l2cap_chan_lock(chan);
6341 } else {
6342 BT_DBG("unknown cid 0x%4.4x", cid);
6343 /* Drop packet and return */
6344 kfree_skb(skb);
13ca56e0 6345 return;
97e8e89d 6346 }
1da177e4
LT		 6347 }
6348
49208c9c 6349 BT_DBG("chan %p, len %d", chan, skb->len);
1da177e4 6350
89bc500e 6351 if (chan->state != BT_CONNECTED)
1da177e4
LT		 6352 goto drop;
6353
0c1bc5c6 6354 switch (chan->mode) {
1c2acffb
GP		 6355 case L2CAP_MODE_BASIC:
6356 /* If socket recv buffers overflows we drop data here
6357 * which is *bad* because L2CAP has to be reliable.
6358 * But we don't have any other choice. L2CAP doesn't
6359 * provide flow control mechanism. */
1da177e4 6360
0c1bc5c6 6361 if (chan->imtu < skb->len)
1c2acffb 6362 goto drop;
1da177e4 6363
80b98027 6364 if (!chan->ops->recv(chan, skb))
1c2acffb
GP		 6365 goto done;
6366 break;
6367
6368 case L2CAP_MODE_ERTM:
6840ed07 6369 case L2CAP_MODE_STREAMING:
cec8ab6e 6370 l2cap_data_rcv(chan, skb);
6840ed07
GP		 6371 goto done;
6372
1c2acffb 6373 default:
0c1bc5c6 6374 BT_DBG("chan %p: bad mode 0x%2.2x", chan, chan->mode);
1c2acffb
GP		 6375 break;
6376 }
1da177e4
LT		 6377
6378drop:
6379 kfree_skb(skb);
6380
6381done:
6be36555 6382 l2cap_chan_unlock(chan);
1da177e4
LT		 6383}
6384
84104b24
AE		 6385static void l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm,
6386 struct sk_buff *skb)
1da177e4 6387{
23691d75 6388 struct l2cap_chan *chan;
1da177e4 6389
c2287681 6390 chan = l2cap_global_chan_by_psm(0, psm, conn->src, conn->dst);
23691d75 6391 if (!chan)
1da177e4
LT		 6392 goto drop;
6393
5b4cedaa 6394 BT_DBG("chan %p, len %d", chan, skb->len);
1da177e4 6395
89bc500e 6396 if (chan->state != BT_BOUND && chan->state != BT_CONNECTED)
1da177e4
LT		 6397 goto drop;
6398
e13e21dc 6399 if (chan->imtu < skb->len)
1da177e4
LT		 6400 goto drop;
6401
80b98027 6402 if (!chan->ops->recv(chan, skb))
84104b24 6403 return;
1da177e4
LT		 6404
6405drop:
6406 kfree_skb(skb);
1da177e4
LT		 6407}
6408
72f78356 6409static void l2cap_att_channel(struct l2cap_conn *conn,
6810fca7 6410 struct sk_buff *skb)
9f69bda6 6411{
23691d75 6412 struct l2cap_chan *chan;
9f69bda6 6413
af1c0134 6414 chan = l2cap_global_chan_by_scid(BT_CONNECTED, L2CAP_CID_ATT,
72f78356 6415 conn->src, conn->dst);
23691d75 6416 if (!chan)
9f69bda6
GP		 6417 goto drop;
6418
5b4cedaa 6419 BT_DBG("chan %p, len %d", chan, skb->len);
9f69bda6 6420
e13e21dc 6421 if (chan->imtu < skb->len)
9f69bda6
GP		 6422 goto drop;
6423
80b98027 6424 if (!chan->ops->recv(chan, skb))
6810fca7 6425 return;
9f69bda6
GP		 6426
6427drop:
6428 kfree_skb(skb);
9f69bda6
GP		 6429}
6430
1da177e4
LT		 6431static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
6432{
6433 struct l2cap_hdr *lh = (void *) skb->data;
8e036fc3
AV		 6434 u16 cid, len;
6435 __le16 psm;
1da177e4
LT		 6436
6437 skb_pull(skb, L2CAP_HDR_SIZE);
6438 cid = __le16_to_cpu(lh->cid);
6439 len = __le16_to_cpu(lh->len);
6440
1c2acffb
GP		 6441 if (len != skb->len) {
6442 kfree_skb(skb);
6443 return;
6444 }
6445
1da177e4
LT		 6446 BT_DBG("len %d, cid 0x%4.4x", len, cid);
6447
6448 switch (cid) {
3300d9a9 6449 case L2CAP_CID_LE_SIGNALING:
c5623556
JH		 6450 l2cap_le_sig_channel(conn, skb);
6451 break;
8db4dc46 6452 case L2CAP_CID_SIGNALING:
1da177e4
LT		 6453 l2cap_sig_channel(conn, skb);
6454 break;
6455
8db4dc46 6456 case L2CAP_CID_CONN_LESS:
097db76c 6457 psm = get_unaligned((__le16 *) skb->data);
0181a70f 6458 skb_pull(skb, L2CAP_PSMLEN_SIZE);
1da177e4
LT		 6459 l2cap_conless_channel(conn, psm, skb);
6460 break;
6461
073d1cf3 6462 case L2CAP_CID_ATT:
72f78356 6463 l2cap_att_channel(conn, skb);
9f69bda6
GP		 6464 break;
6465
b501d6a1
AB		 6466 case L2CAP_CID_SMP:
6467 if (smp_sig_channel(conn, skb))
6468 l2cap_conn_del(conn->hcon, EACCES);
6469 break;
6470
1da177e4
LT		 6471 default:
6472 l2cap_data_channel(conn, cid, skb);
6473 break;
6474 }
6475}
6476
6477/* ---- L2CAP interface with lower layer (HCI) ---- */
6478
686ebf28 6479int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr)
1da177e4
LT		 6480{
6481 int exact = 0, lm1 = 0, lm2 = 0;
23691d75 6482 struct l2cap_chan *c;
1da177e4 6483
6ed93dc6 6484 BT_DBG("hdev %s, bdaddr %pMR", hdev->name, bdaddr);
1da177e4
LT		 6485
6486 /* Find listening sockets and check their link_mode */
23691d75
GP		 6487 read_lock(&chan_list_lock);
6488 list_for_each_entry(c, &chan_list, global_l) {
6489 struct sock *sk = c->sk;
4343478f 6490
89bc500e 6491 if (c->state != BT_LISTEN)
1da177e4
LT		 6492 continue;
6493
6494 if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
2af6b9d5 6495 lm1 |= HCI_LM_ACCEPT;
43bd0f32 6496 if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
2af6b9d5 6497 lm1 |= HCI_LM_MASTER;
1da177e4 6498 exact++;
2af6b9d5
MH		 6499 } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) {
6500 lm2 |= HCI_LM_ACCEPT;
43bd0f32 6501 if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
2af6b9d5
MH		 6502 lm2 |= HCI_LM_MASTER;
6503 }
1da177e4 6504 }
23691d75 6505 read_unlock(&chan_list_lock);
1da177e4
LT		 6506
6507 return exact ? lm1 : lm2;
6508}
6509
9e664631 6510void l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
1da177e4 6511{
0139418c
MH		 6512 struct l2cap_conn *conn;
6513
6ed93dc6 6514 BT_DBG("hcon %p bdaddr %pMR status %d", hcon, &hcon->dst, status);
1da177e4 6515
1da177e4 6516 if (!status) {
baf43251 6517 conn = l2cap_conn_add(hcon);
1da177e4
LT		 6518 if (conn)
6519 l2cap_conn_ready(conn);
ba6fc317 6520 } else {
e175072f 6521 l2cap_conn_del(hcon, bt_to_errno(status));
ba6fc317 6522 }
1da177e4
LT		 6523}
6524
686ebf28 6525int l2cap_disconn_ind(struct hci_conn *hcon)
2950f21a
MH		 6526{
6527 struct l2cap_conn *conn = hcon->l2cap_data;
6528
6529 BT_DBG("hcon %p", hcon);
6530
686ebf28 6531 if (!conn)
9f5a0d7b 6532 return HCI_ERROR_REMOTE_USER_TERM;
2950f21a
MH		 6533 return conn->disc_reason;
6534}
6535
9e664631 6536void l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
1da177e4
LT		 6537{
6538 BT_DBG("hcon %p reason %d", hcon, reason);
6539
e175072f 6540 l2cap_conn_del(hcon, bt_to_errno(reason));
1da177e4
LT		 6541}
6542
4343478f 6543static inline void l2cap_check_encryption(struct l2cap_chan *chan, u8 encrypt)
f62e4323 6544{
715ec005 6545 if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED)
255c7601
MH		 6546 return;
6547
f62e4323 6548 if (encrypt == 0x00) {
4343478f 6549 if (chan->sec_level == BT_SECURITY_MEDIUM) {
ba13ccd9 6550 __set_chan_timer(chan, L2CAP_ENC_TIMEOUT);
4343478f 6551 } else if (chan->sec_level == BT_SECURITY_HIGH)
0f852724 6552 l2cap_chan_close(chan, ECONNREFUSED);
f62e4323 6553 } else {
4343478f 6554 if (chan->sec_level == BT_SECURITY_MEDIUM)
c9b66675 6555 __clear_chan_timer(chan);
f62e4323
MH		 6556 }
6557}
6558
686ebf28 6559int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
1da177e4 6560{
0139418c 6561 struct l2cap_conn *conn = hcon->l2cap_data;
48454079 6562 struct l2cap_chan *chan;
1da177e4 6563
0139418c 6564 if (!conn)
1da177e4 6565 return 0;
0139418c 6566
89d8b407 6567 BT_DBG("conn %p status 0x%2.2x encrypt %u", conn, status, encrypt);
1da177e4 6568
160dc6ac 6569 if (hcon->type == LE_LINK) {
35d4adcc
HG		 6570 if (!status && encrypt)
6571 smp_distribute_keys(conn, 0);
17cd3f37 6572 cancel_delayed_work(&conn->security_timer);
160dc6ac
VCG		 6573 }
6574
3df91ea2 6575 mutex_lock(&conn->chan_lock);
1da177e4 6576
3df91ea2 6577 list_for_each_entry(chan, &conn->chan_l, list) {
6be36555 6578 l2cap_chan_lock(chan);
1da177e4 6579
89d8b407
AE		 6580 BT_DBG("chan %p scid 0x%4.4x state %s", chan, chan->scid,
6581 state_to_string(chan->state));
f1cb9af5 6582
78eb2f98
AE		 6583 if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP) {
6584 l2cap_chan_unlock(chan);
6585 continue;
6586 }
6587
073d1cf3 6588 if (chan->scid == L2CAP_CID_ATT) {
f1cb9af5
VCG		 6589 if (!status && encrypt) {
6590 chan->sec_level = hcon->sec_level;
cf4cd009 6591 l2cap_chan_ready(chan);
f1cb9af5
VCG		 6592 }
6593
6be36555 6594 l2cap_chan_unlock(chan);
f1cb9af5
VCG		 6595 continue;
6596 }
6597
96eff46e 6598 if (!__l2cap_no_conn_pending(chan)) {
6be36555 6599 l2cap_chan_unlock(chan);
6a8d3010
MH		 6600 continue;
6601 }
6602
89bc500e 6603 if (!status && (chan->state == BT_CONNECTED ||
2d792818 6604 chan->state == BT_CONFIG)) {
a7d7723a
GP		 6605 struct sock *sk = chan->sk;
6606
c5daa683 6607 clear_bit(BT_SK_SUSPEND, &bt_sk(sk)->flags);
a7d7723a
GP		 6608 sk->sk_state_change(sk);
6609
4343478f 6610 l2cap_check_encryption(chan, encrypt);
6be36555 6611 l2cap_chan_unlock(chan);
9719f8af
MH		 6612 continue;
6613 }
6614
89bc500e 6615 if (chan->state == BT_CONNECT) {
b1235d79 6616 if (!status) {
93c3e8f5 6617 l2cap_start_connection(chan);
b1235d79 6618 } else {
ba13ccd9 6619 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
b1235d79 6620 }
89bc500e 6621 } else if (chan->state == BT_CONNECT2) {
6be36555 6622 struct sock *sk = chan->sk;
b1235d79 6623 struct l2cap_conn_rsp rsp;
df3c3931 6624 __u16 res, stat;
1da177e4 6625
6be36555
AE		 6626 lock_sock(sk);
6627
b1235d79 6628 if (!status) {
c5daa683
GP		 6629 if (test_bit(BT_SK_DEFER_SETUP,
6630 &bt_sk(sk)->flags)) {
df3c3931
JH		 6631 res = L2CAP_CR_PEND;
6632 stat = L2CAP_CS_AUTHOR_PEND;
2dc4e510 6633 chan->ops->defer(chan);
df3c3931 6634 } else {
0e587be7 6635 __l2cap_state_change(chan, BT_CONFIG);
df3c3931
JH		 6636 res = L2CAP_CR_SUCCESS;
6637 stat = L2CAP_CS_NO_INFO;
6638 }
b1235d79 6639 } else {
0e587be7 6640 __l2cap_state_change(chan, BT_DISCONN);
ba13ccd9 6641 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
df3c3931
JH		 6642 res = L2CAP_CR_SEC_BLOCK;
6643 stat = L2CAP_CS_NO_INFO;
b1235d79
MH		 6644 }
6645
6be36555
AE		 6646 release_sock(sk);
6647
fe4128e0
GP		 6648 rsp.scid = cpu_to_le16(chan->dcid);
6649 rsp.dcid = cpu_to_le16(chan->scid);
df3c3931
JH		 6650 rsp.result = cpu_to_le16(res);
6651 rsp.status = cpu_to_le16(stat);
fc7f8a7e 6652 l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
2d792818 6653 sizeof(rsp), &rsp);
2d369359
MM		 6654
6655 if (!test_bit(CONF_REQ_SENT, &chan->conf_state) &&
6656 res == L2CAP_CR_SUCCESS) {
6657 char buf[128];
6658 set_bit(CONF_REQ_SENT, &chan->conf_state);
6659 l2cap_send_cmd(conn, l2cap_get_ident(conn),
6660 L2CAP_CONF_REQ,
6661 l2cap_build_conf_req(chan, buf),
6662 buf);
6663 chan->num_conf_req++;
6664 }
b1235d79 6665 }
1da177e4 6666
6be36555 6667 l2cap_chan_unlock(chan);
1da177e4
LT		 6668 }
6669
3df91ea2 6670 mutex_unlock(&conn->chan_lock);
b1235d79 6671
1da177e4
LT		 6672 return 0;
6673}
6674
686ebf28 6675int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
1da177e4
LT		 6676{
6677 struct l2cap_conn *conn = hcon->l2cap_data;
d73a0988
AE		 6678 struct l2cap_hdr *hdr;
6679 int len;
1da177e4 6680
1d13a254
AE		 6681 /* For AMP controller do not create l2cap conn */
6682 if (!conn && hcon->hdev->dev_type != HCI_BREDR)
6683 goto drop;
1da177e4 6684
5a08ecce 6685 if (!conn)
baf43251 6686 conn = l2cap_conn_add(hcon);
5a08ecce
AE		 6687
6688 if (!conn)
1da177e4
LT		 6689 goto drop;
6690
6691 BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
6692
d73a0988
AE		 6693 switch (flags) {
6694 case ACL_START:
6695 case ACL_START_NO_FLUSH:
6696 case ACL_COMPLETE:
1da177e4
LT		 6697 if (conn->rx_len) {
6698 BT_ERR("Unexpected start frame (len %d)", skb->len);
6699 kfree_skb(conn->rx_skb);
6700 conn->rx_skb = NULL;
6701 conn->rx_len = 0;
6702 l2cap_conn_unreliable(conn, ECOMM);
6703 }
6704
aae7fe22
AE		 6705 /* Start fragment always begin with Basic L2CAP header */
6706 if (skb->len < L2CAP_HDR_SIZE) {
1da177e4
LT		 6707 BT_ERR("Frame is too short (len %d)", skb->len);
6708 l2cap_conn_unreliable(conn, ECOMM);
6709 goto drop;
6710 }
6711
6712 hdr = (struct l2cap_hdr *) skb->data;
6713 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
6714
6715 if (len == skb->len) {
6716 /* Complete frame received */
6717 l2cap_recv_frame(conn, skb);
6718 return 0;
6719 }
6720
6721 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
6722
6723 if (skb->len > len) {
6724 BT_ERR("Frame is too long (len %d, expected len %d)",
2d792818 6725 skb->len, len);
1da177e4
LT		 6726 l2cap_conn_unreliable(conn, ECOMM);
6727 goto drop;
6728 }
6729
6730 /* Allocate skb for the complete frame (with header) */
8bcde1f2 6731 conn->rx_skb = bt_skb_alloc(len, GFP_KERNEL);
af05b30b 6732 if (!conn->rx_skb)
1da177e4
LT		 6733 goto drop;
6734
d626f62b 6735 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
2d792818 6736 skb->len);
1da177e4 6737 conn->rx_len = len - skb->len;
d73a0988
AE		 6738 break;
6739
6740 case ACL_CONT:
1da177e4
LT		 6741 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
6742
6743 if (!conn->rx_len) {
6744 BT_ERR("Unexpected continuation frame (len %d)", skb->len);
6745 l2cap_conn_unreliable(conn, ECOMM);
6746 goto drop;
6747 }
6748
6749 if (skb->len > conn->rx_len) {
6750 BT_ERR("Fragment is too long (len %d, expected %d)",
2d792818 6751 skb->len, conn->rx_len);
1da177e4
LT		 6752 kfree_skb(conn->rx_skb);
6753 conn->rx_skb = NULL;
6754 conn->rx_len = 0;
6755 l2cap_conn_unreliable(conn, ECOMM);
6756 goto drop;
6757 }
6758
d626f62b 6759 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
2d792818 6760 skb->len);
1da177e4
LT		 6761 conn->rx_len -= skb->len;
6762
6763 if (!conn->rx_len) {
6764 /* Complete frame received */
6765 l2cap_recv_frame(conn, conn->rx_skb);
6766 conn->rx_skb = NULL;
6767 }
d73a0988 6768 break;
1da177e4
LT		 6769 }
6770
6771drop:
6772 kfree_skb(skb);
6773 return 0;
6774}
6775
aef7d97c 6776static int l2cap_debugfs_show(struct seq_file *f, void *p)
1da177e4 6777{
23691d75 6778 struct l2cap_chan *c;
1da177e4 6779
333055f2 6780 read_lock(&chan_list_lock);
1da177e4 6781
23691d75
GP		 6782 list_for_each_entry(c, &chan_list, global_l) {
6783 struct sock *sk = c->sk;
101545f6 6784
fcb73338
AE		 6785 seq_printf(f, "%pMR %pMR %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n",
6786 &bt_sk(sk)->src, &bt_sk(sk)->dst,
6787 c->state, __le16_to_cpu(c->psm),
6788 c->scid, c->dcid, c->imtu, c->omtu,
6789 c->sec_level, c->mode);
61e1b4b7 6790 }
1da177e4 6791
333055f2 6792 read_unlock(&chan_list_lock);
1da177e4 6793
aef7d97c 6794 return 0;
1da177e4
LT		 6795}
6796
aef7d97c
MH		 6797static int l2cap_debugfs_open(struct inode *inode, struct file *file)
6798{
6799 return single_open(file, l2cap_debugfs_show, inode->i_private);
6800}
6801
6802static const struct file_operations l2cap_debugfs_fops = {
6803 .open = l2cap_debugfs_open,
6804 .read = seq_read,
6805 .llseek = seq_lseek,
6806 .release = single_release,
6807};
6808
6809static struct dentry *l2cap_debugfs;
1da177e4 6810
64274518 6811int __init l2cap_init(void)
1da177e4
LT		 6812{
6813 int err;
be9d1227 6814
bb58f747 6815 err = l2cap_init_sockets();
1da177e4
LT		 6816 if (err < 0)
6817 return err;
6818
aef7d97c 6819 if (bt_debugfs) {
2d792818
GP		 6820 l2cap_debugfs = debugfs_create_file("l2cap", 0444, bt_debugfs,
6821 NULL, &l2cap_debugfs_fops);
aef7d97c
MH		 6822 if (!l2cap_debugfs)
6823 BT_ERR("Failed to create L2CAP debug file");
6824 }
1da177e4 6825
1da177e4 6826 return 0;
1da177e4
LT		 6827}
6828
64274518 6829void l2cap_exit(void)
1da177e4 6830{
aef7d97c 6831 debugfs_remove(l2cap_debugfs);
bb58f747 6832 l2cap_cleanup_sockets();
1da177e4
LT		 6833}
6834
d1c4a17d
GP		 6835module_param(disable_ertm, bool, 0644);
6836MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode");
Linux kernel - Deliverables
This page took 1.271279 seconds and 5 git commands to generate.