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