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