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