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