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