?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
x86/speculation/mds: Fix documentation typo
[linux/fpc-iii.git] / net / bluetooth / l2cap_core.c
blobb510da76170ea53cae72f97dd39679e7e01f85df
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, size_t data_size);
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, sizeof(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, size_t size)
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 if (size < L2CAP_CONF_OPT_SIZE + len)
2997 return;
2998
2999 opt->type = type;
3000 opt->len = len;
3001
3002 switch (len) {
3003 case 1:
3004 *((u8 *) opt->val) = val;
3005 break;
3006
3007 case 2:
3008 put_unaligned_le16(val, opt->val);
3009 break;
3010
3011 case 4:
3012 put_unaligned_le32(val, opt->val);
3013 break;
3014
3015 default:
3016 memcpy(opt->val, (void *) val, len);
3017 break;
3018 }
3019
3020 *ptr += L2CAP_CONF_OPT_SIZE + len;
3021 }
3022
3023 static void l2cap_add_opt_efs(void **ptr, struct l2cap_chan *chan, size_t size)
3024 {
3025 struct l2cap_conf_efs efs;
3026
3027 switch (chan->mode) {
3028 case L2CAP_MODE_ERTM:
3029 efs.id = chan->local_id;
3030 efs.stype = chan->local_stype;
3031 efs.msdu = cpu_to_le16(chan->local_msdu);
3032 efs.sdu_itime = cpu_to_le32(chan->local_sdu_itime);
3033 efs.acc_lat = cpu_to_le32(L2CAP_DEFAULT_ACC_LAT);
3034 efs.flush_to = cpu_to_le32(L2CAP_EFS_DEFAULT_FLUSH_TO);
3035 break;
3036
3037 case L2CAP_MODE_STREAMING:
3038 efs.id = 1;
3039 efs.stype = L2CAP_SERV_BESTEFFORT;
3040 efs.msdu = cpu_to_le16(chan->local_msdu);
3041 efs.sdu_itime = cpu_to_le32(chan->local_sdu_itime);
3042 efs.acc_lat = 0;
3043 efs.flush_to = 0;
3044 break;
3045
3046 default:
3047 return;
3048 }
3049
3050 l2cap_add_conf_opt(ptr, L2CAP_CONF_EFS, sizeof(efs),
3051 (unsigned long) &efs, size);
3052 }
3053
3054 static void l2cap_ack_timeout(struct work_struct *work)
3055 {
3056 struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
3057 ack_timer.work);
3058 u16 frames_to_ack;
3059
3060 BT_DBG("chan %p", chan);
3061
3062 l2cap_chan_lock(chan);
3063
3064 frames_to_ack = __seq_offset(chan, chan->buffer_seq,
3065 chan->last_acked_seq);
3066
3067 if (frames_to_ack)
3068 l2cap_send_rr_or_rnr(chan, 0);
3069
3070 l2cap_chan_unlock(chan);
3071 l2cap_chan_put(chan);
3072 }
3073
3074 int l2cap_ertm_init(struct l2cap_chan *chan)
3075 {
3076 int err;
3077
3078 chan->next_tx_seq = 0;
3079 chan->expected_tx_seq = 0;
3080 chan->expected_ack_seq = 0;
3081 chan->unacked_frames = 0;
3082 chan->buffer_seq = 0;
3083 chan->frames_sent = 0;
3084 chan->last_acked_seq = 0;
3085 chan->sdu = NULL;
3086 chan->sdu_last_frag = NULL;
3087 chan->sdu_len = 0;
3088
3089 skb_queue_head_init(&chan->tx_q);
3090
3091 chan->local_amp_id = AMP_ID_BREDR;
3092 chan->move_id = AMP_ID_BREDR;
3093 chan->move_state = L2CAP_MOVE_STABLE;
3094 chan->move_role = L2CAP_MOVE_ROLE_NONE;
3095
3096 if (chan->mode != L2CAP_MODE_ERTM)
3097 return 0;
3098
3099 chan->rx_state = L2CAP_RX_STATE_RECV;
3100 chan->tx_state = L2CAP_TX_STATE_XMIT;
3101
3102 INIT_DELAYED_WORK(&chan->retrans_timer, l2cap_retrans_timeout);
3103 INIT_DELAYED_WORK(&chan->monitor_timer, l2cap_monitor_timeout);
3104 INIT_DELAYED_WORK(&chan->ack_timer, l2cap_ack_timeout);
3105
3106 skb_queue_head_init(&chan->srej_q);
3107
3108 err = l2cap_seq_list_init(&chan->srej_list, chan->tx_win);
3109 if (err < 0)
3110 return err;
3111
3112 err = l2cap_seq_list_init(&chan->retrans_list, chan->remote_tx_win);
3113 if (err < 0)
3114 l2cap_seq_list_free(&chan->srej_list);
3115
3116 return err;
3117 }
3118
3119 static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask)
3120 {
3121 switch (mode) {
3122 case L2CAP_MODE_STREAMING:
3123 case L2CAP_MODE_ERTM:
3124 if (l2cap_mode_supported(mode, remote_feat_mask))
3125 return mode;
3126 /* fall through */
3127 default:
3128 return L2CAP_MODE_BASIC;
3129 }
3130 }
3131
3132 static inline bool __l2cap_ews_supported(struct l2cap_conn *conn)
3133 {
3134 return ((conn->local_fixed_chan & L2CAP_FC_A2MP) &&
3135 (conn->feat_mask & L2CAP_FEAT_EXT_WINDOW));
3136 }
3137
3138 static inline bool __l2cap_efs_supported(struct l2cap_conn *conn)
3139 {
3140 return ((conn->local_fixed_chan & L2CAP_FC_A2MP) &&
3141 (conn->feat_mask & L2CAP_FEAT_EXT_FLOW));
3142 }
3143
3144 static void __l2cap_set_ertm_timeouts(struct l2cap_chan *chan,
3145 struct l2cap_conf_rfc *rfc)
3146 {
3147 if (chan->local_amp_id != AMP_ID_BREDR && chan->hs_hcon) {
3148 u64 ertm_to = chan->hs_hcon->hdev->amp_be_flush_to;
3149
3150 /* Class 1 devices have must have ERTM timeouts
3151 * exceeding the Link Supervision Timeout. The
3152 * default Link Supervision Timeout for AMP
3153 * controllers is 10 seconds.
3154 *
3155 * Class 1 devices use 0xffffffff for their
3156 * best-effort flush timeout, so the clamping logic
3157 * will result in a timeout that meets the above
3158 * requirement. ERTM timeouts are 16-bit values, so
3159 * the maximum timeout is 65.535 seconds.
3160 */
3161
3162 /* Convert timeout to milliseconds and round */
3163 ertm_to = DIV_ROUND_UP_ULL(ertm_to, 1000);
3164
3165 /* This is the recommended formula for class 2 devices
3166 * that start ERTM timers when packets are sent to the
3167 * controller.
3168 */
3169 ertm_to = 3 * ertm_to + 500;
3170
3171 if (ertm_to > 0xffff)
3172 ertm_to = 0xffff;
3173
3174 rfc->retrans_timeout = cpu_to_le16((u16) ertm_to);
3175 rfc->monitor_timeout = rfc->retrans_timeout;
3176 } else {
3177 rfc->retrans_timeout = cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO);
3178 rfc->monitor_timeout = cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO);
3179 }
3180 }
3181
3182 static inline void l2cap_txwin_setup(struct l2cap_chan *chan)
3183 {
3184 if (chan->tx_win > L2CAP_DEFAULT_TX_WINDOW &&
3185 __l2cap_ews_supported(chan->conn)) {
3186 /* use extended control field */
3187 set_bit(FLAG_EXT_CTRL, &chan->flags);
3188 chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
3189 } else {
3190 chan->tx_win = min_t(u16, chan->tx_win,
3191 L2CAP_DEFAULT_TX_WINDOW);
3192 chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW;
3193 }
3194 chan->ack_win = chan->tx_win;
3195 }
3196
3197 static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data, size_t data_size)
3198 {
3199 struct l2cap_conf_req *req = data;
3200 struct l2cap_conf_rfc rfc = { .mode = chan->mode };
3201 void *ptr = req->data;
3202 void *endptr = data + data_size;
3203 u16 size;
3204
3205 BT_DBG("chan %p", chan);
3206
3207 if (chan->num_conf_req || chan->num_conf_rsp)
3208 goto done;
3209
3210 switch (chan->mode) {
3211 case L2CAP_MODE_STREAMING:
3212 case L2CAP_MODE_ERTM:
3213 if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state))
3214 break;
3215
3216 if (__l2cap_efs_supported(chan->conn))
3217 set_bit(FLAG_EFS_ENABLE, &chan->flags);
3218
3219 /* fall through */
3220 default:
3221 chan->mode = l2cap_select_mode(rfc.mode, chan->conn->feat_mask);
3222 break;
3223 }
3224
3225 done:
3226 if (chan->imtu != L2CAP_DEFAULT_MTU)
3227 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu, endptr - ptr);
3228
3229 switch (chan->mode) {
3230 case L2CAP_MODE_BASIC:
3231 if (disable_ertm)
3232 break;
3233
3234 if (!(chan->conn->feat_mask & L2CAP_FEAT_ERTM) &&
3235 !(chan->conn->feat_mask & L2CAP_FEAT_STREAMING))
3236 break;
3237
3238 rfc.mode = L2CAP_MODE_BASIC;
3239 rfc.txwin_size = 0;
3240 rfc.max_transmit = 0;
3241 rfc.retrans_timeout = 0;
3242 rfc.monitor_timeout = 0;
3243 rfc.max_pdu_size = 0;
3244
3245 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3246 (unsigned long) &rfc, endptr - ptr);
3247 break;
3248
3249 case L2CAP_MODE_ERTM:
3250 rfc.mode = L2CAP_MODE_ERTM;
3251 rfc.max_transmit = chan->max_tx;
3252
3253 __l2cap_set_ertm_timeouts(chan, &rfc);
3254
3255 size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
3256 L2CAP_EXT_HDR_SIZE - L2CAP_SDULEN_SIZE -
3257 L2CAP_FCS_SIZE);
3258 rfc.max_pdu_size = cpu_to_le16(size);
3259
3260 l2cap_txwin_setup(chan);
3261
3262 rfc.txwin_size = min_t(u16, chan->tx_win,
3263 L2CAP_DEFAULT_TX_WINDOW);
3264
3265 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3266 (unsigned long) &rfc, endptr - ptr);
3267
3268 if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
3269 l2cap_add_opt_efs(&ptr, chan, endptr - ptr);
3270
3271 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
3272 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
3273 chan->tx_win, endptr - ptr);
3274
3275 if (chan->conn->feat_mask & L2CAP_FEAT_FCS)
3276 if (chan->fcs == L2CAP_FCS_NONE ||
3277 test_bit(CONF_RECV_NO_FCS, &chan->conf_state)) {
3278 chan->fcs = L2CAP_FCS_NONE;
3279 l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1,
3280 chan->fcs, endptr - ptr);
3281 }
3282 break;
3283
3284 case L2CAP_MODE_STREAMING:
3285 l2cap_txwin_setup(chan);
3286 rfc.mode = L2CAP_MODE_STREAMING;
3287 rfc.txwin_size = 0;
3288 rfc.max_transmit = 0;
3289 rfc.retrans_timeout = 0;
3290 rfc.monitor_timeout = 0;
3291
3292 size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
3293 L2CAP_EXT_HDR_SIZE - L2CAP_SDULEN_SIZE -
3294 L2CAP_FCS_SIZE);
3295 rfc.max_pdu_size = cpu_to_le16(size);
3296
3297 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3298 (unsigned long) &rfc, endptr - ptr);
3299
3300 if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
3301 l2cap_add_opt_efs(&ptr, chan, endptr - ptr);
3302
3303 if (chan->conn->feat_mask & L2CAP_FEAT_FCS)
3304 if (chan->fcs == L2CAP_FCS_NONE ||
3305 test_bit(CONF_RECV_NO_FCS, &chan->conf_state)) {
3306 chan->fcs = L2CAP_FCS_NONE;
3307 l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1,
3308 chan->fcs, endptr - ptr);
3309 }
3310 break;
3311 }
3312
3313 req->dcid = cpu_to_le16(chan->dcid);
3314 req->flags = cpu_to_le16(0);
3315
3316 return ptr - data;
3317 }
3318
3319 static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data, size_t data_size)
3320 {
3321 struct l2cap_conf_rsp *rsp = data;
3322 void *ptr = rsp->data;
3323 void *endptr = data + data_size;
3324 void *req = chan->conf_req;
3325 int len = chan->conf_len;
3326 int type, hint, olen;
3327 unsigned long val;
3328 struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
3329 struct l2cap_conf_efs efs;
3330 u8 remote_efs = 0;
3331 u16 mtu = L2CAP_DEFAULT_MTU;
3332 u16 result = L2CAP_CONF_SUCCESS;
3333 u16 size;
3334
3335 BT_DBG("chan %p", chan);
3336
3337 while (len >= L2CAP_CONF_OPT_SIZE) {
3338 len -= l2cap_get_conf_opt(&req, &type, &olen, &val);
3339 if (len < 0)
3340 break;
3341
3342 hint = type & L2CAP_CONF_HINT;
3343 type &= L2CAP_CONF_MASK;
3344
3345 switch (type) {
3346 case L2CAP_CONF_MTU:
3347 if (olen != 2)
3348 break;
3349 mtu = val;
3350 break;
3351
3352 case L2CAP_CONF_FLUSH_TO:
3353 if (olen != 2)
3354 break;
3355 chan->flush_to = val;
3356 break;
3357
3358 case L2CAP_CONF_QOS:
3359 break;
3360
3361 case L2CAP_CONF_RFC:
3362 if (olen != sizeof(rfc))
3363 break;
3364 memcpy(&rfc, (void *) val, olen);
3365 break;
3366
3367 case L2CAP_CONF_FCS:
3368 if (olen != 1)
3369 break;
3370 if (val == L2CAP_FCS_NONE)
3371 set_bit(CONF_RECV_NO_FCS, &chan->conf_state);
3372 break;
3373
3374 case L2CAP_CONF_EFS:
3375 if (olen != sizeof(efs))
3376 break;
3377 remote_efs = 1;
3378 memcpy(&efs, (void *) val, olen);
3379 break;
3380
3381 case L2CAP_CONF_EWS:
3382 if (olen != 2)
3383 break;
3384 if (!(chan->conn->local_fixed_chan & L2CAP_FC_A2MP))
3385 return -ECONNREFUSED;
3386 set_bit(FLAG_EXT_CTRL, &chan->flags);
3387 set_bit(CONF_EWS_RECV, &chan->conf_state);
3388 chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
3389 chan->remote_tx_win = val;
3390 break;
3391
3392 default:
3393 if (hint)
3394 break;
3395 result = L2CAP_CONF_UNKNOWN;
3396 *((u8 *) ptr++) = type;
3397 break;
3398 }
3399 }
3400
3401 if (chan->num_conf_rsp || chan->num_conf_req > 1)
3402 goto done;
3403
3404 switch (chan->mode) {
3405 case L2CAP_MODE_STREAMING:
3406 case L2CAP_MODE_ERTM:
3407 if (!test_bit(CONF_STATE2_DEVICE, &chan->conf_state)) {
3408 chan->mode = l2cap_select_mode(rfc.mode,
3409 chan->conn->feat_mask);
3410 break;
3411 }
3412
3413 if (remote_efs) {
3414 if (__l2cap_efs_supported(chan->conn))
3415 set_bit(FLAG_EFS_ENABLE, &chan->flags);
3416 else
3417 return -ECONNREFUSED;
3418 }
3419
3420 if (chan->mode != rfc.mode)
3421 return -ECONNREFUSED;
3422
3423 break;
3424 }
3425
3426 done:
3427 if (chan->mode != rfc.mode) {
3428 result = L2CAP_CONF_UNACCEPT;
3429 rfc.mode = chan->mode;
3430
3431 if (chan->num_conf_rsp == 1)
3432 return -ECONNREFUSED;
3433
3434 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3435 (unsigned long) &rfc, endptr - ptr);
3436 }
3437
3438 if (result == L2CAP_CONF_SUCCESS) {
3439 /* Configure output options and let the other side know
3440 * which ones we don't like. */
3441
3442 if (mtu < L2CAP_DEFAULT_MIN_MTU)
3443 result = L2CAP_CONF_UNACCEPT;
3444 else {
3445 chan->omtu = mtu;
3446 set_bit(CONF_MTU_DONE, &chan->conf_state);
3447 }
3448 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->omtu, endptr - ptr);
3449
3450 if (remote_efs) {
3451 if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
3452 efs.stype != L2CAP_SERV_NOTRAFIC &&
3453 efs.stype != chan->local_stype) {
3454
3455 result = L2CAP_CONF_UNACCEPT;
3456
3457 if (chan->num_conf_req >= 1)
3458 return -ECONNREFUSED;
3459
3460 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
3461 sizeof(efs),
3462 (unsigned long) &efs, endptr - ptr);
3463 } else {
3464 /* Send PENDING Conf Rsp */
3465 result = L2CAP_CONF_PENDING;
3466 set_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
3467 }
3468 }
3469
3470 switch (rfc.mode) {
3471 case L2CAP_MODE_BASIC:
3472 chan->fcs = L2CAP_FCS_NONE;
3473 set_bit(CONF_MODE_DONE, &chan->conf_state);
3474 break;
3475
3476 case L2CAP_MODE_ERTM:
3477 if (!test_bit(CONF_EWS_RECV, &chan->conf_state))
3478 chan->remote_tx_win = rfc.txwin_size;
3479 else
3480 rfc.txwin_size = L2CAP_DEFAULT_TX_WINDOW;
3481
3482 chan->remote_max_tx = rfc.max_transmit;
3483
3484 size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
3485 chan->conn->mtu - L2CAP_EXT_HDR_SIZE -
3486 L2CAP_SDULEN_SIZE - L2CAP_FCS_SIZE);
3487 rfc.max_pdu_size = cpu_to_le16(size);
3488 chan->remote_mps = size;
3489
3490 __l2cap_set_ertm_timeouts(chan, &rfc);
3491
3492 set_bit(CONF_MODE_DONE, &chan->conf_state);
3493
3494 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
3495 sizeof(rfc), (unsigned long) &rfc, endptr - ptr);
3496
3497 if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
3498 chan->remote_id = efs.id;
3499 chan->remote_stype = efs.stype;
3500 chan->remote_msdu = le16_to_cpu(efs.msdu);
3501 chan->remote_flush_to =
3502 le32_to_cpu(efs.flush_to);
3503 chan->remote_acc_lat =
3504 le32_to_cpu(efs.acc_lat);
3505 chan->remote_sdu_itime =
3506 le32_to_cpu(efs.sdu_itime);
3507 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
3508 sizeof(efs),
3509 (unsigned long) &efs, endptr - ptr);
3510 }
3511 break;
3512
3513 case L2CAP_MODE_STREAMING:
3514 size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
3515 chan->conn->mtu - L2CAP_EXT_HDR_SIZE -
3516 L2CAP_SDULEN_SIZE - L2CAP_FCS_SIZE);
3517 rfc.max_pdu_size = cpu_to_le16(size);
3518 chan->remote_mps = size;
3519
3520 set_bit(CONF_MODE_DONE, &chan->conf_state);
3521
3522 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3523 (unsigned long) &rfc, endptr - ptr);
3524
3525 break;
3526
3527 default:
3528 result = L2CAP_CONF_UNACCEPT;
3529
3530 memset(&rfc, 0, sizeof(rfc));
3531 rfc.mode = chan->mode;
3532 }
3533
3534 if (result == L2CAP_CONF_SUCCESS)
3535 set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
3536 }
3537 rsp->scid = cpu_to_le16(chan->dcid);
3538 rsp->result = cpu_to_le16(result);
3539 rsp->flags = cpu_to_le16(0);
3540
3541 return ptr - data;
3542 }
3543
3544 static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len,
3545 void *data, size_t size, u16 *result)
3546 {
3547 struct l2cap_conf_req *req = data;
3548 void *ptr = req->data;
3549 void *endptr = data + size;
3550 int type, olen;
3551 unsigned long val;
3552 struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
3553 struct l2cap_conf_efs efs;
3554
3555 BT_DBG("chan %p, rsp %p, len %d, req %p", chan, rsp, len, data);
3556
3557 while (len >= L2CAP_CONF_OPT_SIZE) {
3558 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
3559 if (len < 0)
3560 break;
3561
3562 switch (type) {
3563 case L2CAP_CONF_MTU:
3564 if (olen != 2)
3565 break;
3566 if (val < L2CAP_DEFAULT_MIN_MTU) {
3567 *result = L2CAP_CONF_UNACCEPT;
3568 chan->imtu = L2CAP_DEFAULT_MIN_MTU;
3569 } else
3570 chan->imtu = val;
3571 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu,
3572 endptr - ptr);
3573 break;
3574
3575 case L2CAP_CONF_FLUSH_TO:
3576 if (olen != 2)
3577 break;
3578 chan->flush_to = val;
3579 l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO, 2,
3580 chan->flush_to, endptr - ptr);
3581 break;
3582
3583 case L2CAP_CONF_RFC:
3584 if (olen != sizeof(rfc))
3585 break;
3586 memcpy(&rfc, (void *)val, olen);
3587 if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state) &&
3588 rfc.mode != chan->mode)
3589 return -ECONNREFUSED;
3590 chan->fcs = 0;
3591 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3592 (unsigned long) &rfc, endptr - ptr);
3593 break;
3594
3595 case L2CAP_CONF_EWS:
3596 if (olen != 2)
3597 break;
3598 chan->ack_win = min_t(u16, val, chan->ack_win);
3599 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
3600 chan->tx_win, endptr - ptr);
3601 break;
3602
3603 case L2CAP_CONF_EFS:
3604 if (olen != sizeof(efs))
3605 break;
3606 memcpy(&efs, (void *)val, olen);
3607 if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
3608 efs.stype != L2CAP_SERV_NOTRAFIC &&
3609 efs.stype != chan->local_stype)
3610 return -ECONNREFUSED;
3611 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS, sizeof(efs),
3612 (unsigned long) &efs, endptr - ptr);
3613 break;
3614
3615 case L2CAP_CONF_FCS:
3616 if (olen != 1)
3617 break;
3618 if (*result == L2CAP_CONF_PENDING)
3619 if (val == L2CAP_FCS_NONE)
3620 set_bit(CONF_RECV_NO_FCS,
3621 &chan->conf_state);
3622 break;
3623 }
3624 }
3625
3626 if (chan->mode == L2CAP_MODE_BASIC && chan->mode != rfc.mode)
3627 return -ECONNREFUSED;
3628
3629 chan->mode = rfc.mode;
3630
3631 if (*result == L2CAP_CONF_SUCCESS || *result == L2CAP_CONF_PENDING) {
3632 switch (rfc.mode) {
3633 case L2CAP_MODE_ERTM:
3634 chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
3635 chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
3636 chan->mps = le16_to_cpu(rfc.max_pdu_size);
3637 if (!test_bit(FLAG_EXT_CTRL, &chan->flags))
3638 chan->ack_win = min_t(u16, chan->ack_win,
3639 rfc.txwin_size);
3640
3641 if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
3642 chan->local_msdu = le16_to_cpu(efs.msdu);
3643 chan->local_sdu_itime =
3644 le32_to_cpu(efs.sdu_itime);
3645 chan->local_acc_lat = le32_to_cpu(efs.acc_lat);
3646 chan->local_flush_to =
3647 le32_to_cpu(efs.flush_to);
3648 }
3649 break;
3650
3651 case L2CAP_MODE_STREAMING:
3652 chan->mps = le16_to_cpu(rfc.max_pdu_size);
3653 }
3654 }
3655
3656 req->dcid = cpu_to_le16(chan->dcid);
3657 req->flags = cpu_to_le16(0);
3658
3659 return ptr - data;
3660 }
3661
3662 static int l2cap_build_conf_rsp(struct l2cap_chan *chan, void *data,
3663 u16 result, u16 flags)
3664 {
3665 struct l2cap_conf_rsp *rsp = data;
3666 void *ptr = rsp->data;
3667
3668 BT_DBG("chan %p", chan);
3669
3670 rsp->scid = cpu_to_le16(chan->dcid);
3671 rsp->result = cpu_to_le16(result);
3672 rsp->flags = cpu_to_le16(flags);
3673
3674 return ptr - data;
3675 }
3676
3677 void __l2cap_le_connect_rsp_defer(struct l2cap_chan *chan)
3678 {
3679 struct l2cap_le_conn_rsp rsp;
3680 struct l2cap_conn *conn = chan->conn;
3681
3682 BT_DBG("chan %p", chan);
3683
3684 rsp.dcid = cpu_to_le16(chan->scid);
3685 rsp.mtu = cpu_to_le16(chan->imtu);
3686 rsp.mps = cpu_to_le16(chan->mps);
3687 rsp.credits = cpu_to_le16(chan->rx_credits);
3688 rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
3689
3690 l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CONN_RSP, sizeof(rsp),
3691 &rsp);
3692 }
3693
3694 void __l2cap_connect_rsp_defer(struct l2cap_chan *chan)
3695 {
3696 struct l2cap_conn_rsp rsp;
3697 struct l2cap_conn *conn = chan->conn;
3698 u8 buf[128];
3699 u8 rsp_code;
3700
3701 rsp.scid = cpu_to_le16(chan->dcid);
3702 rsp.dcid = cpu_to_le16(chan->scid);
3703 rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
3704 rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
3705
3706 if (chan->hs_hcon)
3707 rsp_code = L2CAP_CREATE_CHAN_RSP;
3708 else
3709 rsp_code = L2CAP_CONN_RSP;
3710
3711 BT_DBG("chan %p rsp_code %u", chan, rsp_code);
3712
3713 l2cap_send_cmd(conn, chan->ident, rsp_code, sizeof(rsp), &rsp);
3714
3715 if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
3716 return;
3717
3718 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3719 l2cap_build_conf_req(chan, buf, sizeof(buf)), buf);
3720 chan->num_conf_req++;
3721 }
3722
3723 static void l2cap_conf_rfc_get(struct l2cap_chan *chan, void *rsp, int len)
3724 {
3725 int type, olen;
3726 unsigned long val;
3727 /* Use sane default values in case a misbehaving remote device
3728 * did not send an RFC or extended window size option.
3729 */
3730 u16 txwin_ext = chan->ack_win;
3731 struct l2cap_conf_rfc rfc = {
3732 .mode = chan->mode,
3733 .retrans_timeout = cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO),
3734 .monitor_timeout = cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO),
3735 .max_pdu_size = cpu_to_le16(chan->imtu),
3736 .txwin_size = min_t(u16, chan->ack_win, L2CAP_DEFAULT_TX_WINDOW),
3737 };
3738
3739 BT_DBG("chan %p, rsp %p, len %d", chan, rsp, len);
3740
3741 if ((chan->mode != L2CAP_MODE_ERTM) && (chan->mode != L2CAP_MODE_STREAMING))
3742 return;
3743
3744 while (len >= L2CAP_CONF_OPT_SIZE) {
3745 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
3746 if (len < 0)
3747 break;
3748
3749 switch (type) {
3750 case L2CAP_CONF_RFC:
3751 if (olen != sizeof(rfc))
3752 break;
3753 memcpy(&rfc, (void *)val, olen);
3754 break;
3755 case L2CAP_CONF_EWS:
3756 if (olen != 2)
3757 break;
3758 txwin_ext = val;
3759 break;
3760 }
3761 }
3762
3763 switch (rfc.mode) {
3764 case L2CAP_MODE_ERTM:
3765 chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
3766 chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
3767 chan->mps = le16_to_cpu(rfc.max_pdu_size);
3768 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
3769 chan->ack_win = min_t(u16, chan->ack_win, txwin_ext);
3770 else
3771 chan->ack_win = min_t(u16, chan->ack_win,
3772 rfc.txwin_size);
3773 break;
3774 case L2CAP_MODE_STREAMING:
3775 chan->mps = le16_to_cpu(rfc.max_pdu_size);
3776 }
3777 }
3778
3779 static inline int l2cap_command_rej(struct l2cap_conn *conn,
3780 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
3781 u8 *data)
3782 {
3783 struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data;
3784
3785 if (cmd_len < sizeof(*rej))
3786 return -EPROTO;
3787
3788 if (rej->reason != L2CAP_REJ_NOT_UNDERSTOOD)
3789 return 0;
3790
3791 if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
3792 cmd->ident == conn->info_ident) {
3793 cancel_delayed_work(&conn->info_timer);
3794
3795 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3796 conn->info_ident = 0;
3797
3798 l2cap_conn_start(conn);
3799 }
3800
3801 return 0;
3802 }
3803
3804 static struct l2cap_chan *l2cap_connect(struct l2cap_conn *conn,
3805 struct l2cap_cmd_hdr *cmd,
3806 u8 *data, u8 rsp_code, u8 amp_id)
3807 {
3808 struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
3809 struct l2cap_conn_rsp rsp;
3810 struct l2cap_chan *chan = NULL, *pchan;
3811 int result, status = L2CAP_CS_NO_INFO;
3812
3813 u16 dcid = 0, scid = __le16_to_cpu(req->scid);
3814 __le16 psm = req->psm;
3815
3816 BT_DBG("psm 0x%2.2x scid 0x%4.4x", __le16_to_cpu(psm), scid);
3817
3818 /* Check if we have socket listening on psm */
3819 pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, &conn->hcon->src,
3820 &conn->hcon->dst, ACL_LINK);
3821 if (!pchan) {
3822 result = L2CAP_CR_BAD_PSM;
3823 goto sendresp;
3824 }
3825
3826 mutex_lock(&conn->chan_lock);
3827 l2cap_chan_lock(pchan);
3828
3829 /* Check if the ACL is secure enough (if not SDP) */
3830 if (psm != cpu_to_le16(L2CAP_PSM_SDP) &&
3831 !hci_conn_check_link_mode(conn->hcon)) {
3832 conn->disc_reason = HCI_ERROR_AUTH_FAILURE;
3833 result = L2CAP_CR_SEC_BLOCK;
3834 goto response;
3835 }
3836
3837 result = L2CAP_CR_NO_MEM;
3838
3839 /* Check if we already have channel with that dcid */
3840 if (__l2cap_get_chan_by_dcid(conn, scid))
3841 goto response;
3842
3843 chan = pchan->ops->new_connection(pchan);
3844 if (!chan)
3845 goto response;
3846
3847 /* For certain devices (ex: HID mouse), support for authentication,
3848 * pairing and bonding is optional. For such devices, inorder to avoid
3849 * the ACL alive for too long after L2CAP disconnection, reset the ACL
3850 * disc_timeout back to HCI_DISCONN_TIMEOUT during L2CAP connect.
3851 */
3852 conn->hcon->disc_timeout = HCI_DISCONN_TIMEOUT;
3853
3854 bacpy(&chan->src, &conn->hcon->src);
3855 bacpy(&chan->dst, &conn->hcon->dst);
3856 chan->src_type = bdaddr_src_type(conn->hcon);
3857 chan->dst_type = bdaddr_dst_type(conn->hcon);
3858 chan->psm = psm;
3859 chan->dcid = scid;
3860 chan->local_amp_id = amp_id;
3861
3862 __l2cap_chan_add(conn, chan);
3863
3864 dcid = chan->scid;
3865
3866 __set_chan_timer(chan, chan->ops->get_sndtimeo(chan));
3867
3868 chan->ident = cmd->ident;
3869
3870 if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
3871 if (l2cap_chan_check_security(chan, false)) {
3872 if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
3873 l2cap_state_change(chan, BT_CONNECT2);
3874 result = L2CAP_CR_PEND;
3875 status = L2CAP_CS_AUTHOR_PEND;
3876 chan->ops->defer(chan);
3877 } else {
3878 /* Force pending result for AMP controllers.
3879 * The connection will succeed after the
3880 * physical link is up.
3881 */
3882 if (amp_id == AMP_ID_BREDR) {
3883 l2cap_state_change(chan, BT_CONFIG);
3884 result = L2CAP_CR_SUCCESS;
3885 } else {
3886 l2cap_state_change(chan, BT_CONNECT2);
3887 result = L2CAP_CR_PEND;
3888 }
3889 status = L2CAP_CS_NO_INFO;
3890 }
3891 } else {
3892 l2cap_state_change(chan, BT_CONNECT2);
3893 result = L2CAP_CR_PEND;
3894 status = L2CAP_CS_AUTHEN_PEND;
3895 }
3896 } else {
3897 l2cap_state_change(chan, BT_CONNECT2);
3898 result = L2CAP_CR_PEND;
3899 status = L2CAP_CS_NO_INFO;
3900 }
3901
3902 response:
3903 l2cap_chan_unlock(pchan);
3904 mutex_unlock(&conn->chan_lock);
3905 l2cap_chan_put(pchan);
3906
3907 sendresp:
3908 rsp.scid = cpu_to_le16(scid);
3909 rsp.dcid = cpu_to_le16(dcid);
3910 rsp.result = cpu_to_le16(result);
3911 rsp.status = cpu_to_le16(status);
3912 l2cap_send_cmd(conn, cmd->ident, rsp_code, sizeof(rsp), &rsp);
3913
3914 if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
3915 struct l2cap_info_req info;
3916 info.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
3917
3918 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
3919 conn->info_ident = l2cap_get_ident(conn);
3920
3921 schedule_delayed_work(&conn->info_timer, L2CAP_INFO_TIMEOUT);
3922
3923 l2cap_send_cmd(conn, conn->info_ident, L2CAP_INFO_REQ,
3924 sizeof(info), &info);
3925 }
3926
3927 if (chan && !test_bit(CONF_REQ_SENT, &chan->conf_state) &&
3928 result == L2CAP_CR_SUCCESS) {
3929 u8 buf[128];
3930 set_bit(CONF_REQ_SENT, &chan->conf_state);
3931 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3932 l2cap_build_conf_req(chan, buf, sizeof(buf)), buf);
3933 chan->num_conf_req++;
3934 }
3935
3936 return chan;
3937 }
3938
3939 static int l2cap_connect_req(struct l2cap_conn *conn,
3940 struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
3941 {
3942 struct hci_dev *hdev = conn->hcon->hdev;
3943 struct hci_conn *hcon = conn->hcon;
3944
3945 if (cmd_len < sizeof(struct l2cap_conn_req))
3946 return -EPROTO;
3947
3948 hci_dev_lock(hdev);
3949 if (hci_dev_test_flag(hdev, HCI_MGMT) &&
3950 !test_and_set_bit(HCI_CONN_MGMT_CONNECTED, &hcon->flags))
3951 mgmt_device_connected(hdev, hcon, 0, NULL, 0);
3952 hci_dev_unlock(hdev);
3953
3954 l2cap_connect(conn, cmd, data, L2CAP_CONN_RSP, 0);
3955 return 0;
3956 }
3957
3958 static int l2cap_connect_create_rsp(struct l2cap_conn *conn,
3959 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
3960 u8 *data)
3961 {
3962 struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
3963 u16 scid, dcid, result, status;
3964 struct l2cap_chan *chan;
3965 u8 req[128];
3966 int err;
3967
3968 if (cmd_len < sizeof(*rsp))
3969 return -EPROTO;
3970
3971 scid = __le16_to_cpu(rsp->scid);
3972 dcid = __le16_to_cpu(rsp->dcid);
3973 result = __le16_to_cpu(rsp->result);
3974 status = __le16_to_cpu(rsp->status);
3975
3976 BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x",
3977 dcid, scid, result, status);
3978
3979 mutex_lock(&conn->chan_lock);
3980
3981 if (scid) {
3982 chan = __l2cap_get_chan_by_scid(conn, scid);
3983 if (!chan) {
3984 err = -EBADSLT;
3985 goto unlock;
3986 }
3987 } else {
3988 chan = __l2cap_get_chan_by_ident(conn, cmd->ident);
3989 if (!chan) {
3990 err = -EBADSLT;
3991 goto unlock;
3992 }
3993 }
3994
3995 err = 0;
3996
3997 l2cap_chan_lock(chan);
3998
3999 switch (result) {
4000 case L2CAP_CR_SUCCESS:
4001 l2cap_state_change(chan, BT_CONFIG);
4002 chan->ident = 0;
4003 chan->dcid = dcid;
4004 clear_bit(CONF_CONNECT_PEND, &chan->conf_state);
4005
4006 if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
4007 break;
4008
4009 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
4010 l2cap_build_conf_req(chan, req, sizeof(req)), req);
4011 chan->num_conf_req++;
4012 break;
4013
4014 case L2CAP_CR_PEND:
4015 set_bit(CONF_CONNECT_PEND, &chan->conf_state);
4016 break;
4017
4018 default:
4019 l2cap_chan_del(chan, ECONNREFUSED);
4020 break;
4021 }
4022
4023 l2cap_chan_unlock(chan);
4024
4025 unlock:
4026 mutex_unlock(&conn->chan_lock);
4027
4028 return err;
4029 }
4030
4031 static inline void set_default_fcs(struct l2cap_chan *chan)
4032 {
4033 /* FCS is enabled only in ERTM or streaming mode, if one or both
4034 * sides request it.
4035 */
4036 if (chan->mode != L2CAP_MODE_ERTM && chan->mode != L2CAP_MODE_STREAMING)
4037 chan->fcs = L2CAP_FCS_NONE;
4038 else if (!test_bit(CONF_RECV_NO_FCS, &chan->conf_state))
4039 chan->fcs = L2CAP_FCS_CRC16;
4040 }
4041
4042 static void l2cap_send_efs_conf_rsp(struct l2cap_chan *chan, void *data,
4043 u8 ident, u16 flags)
4044 {
4045 struct l2cap_conn *conn = chan->conn;
4046
4047 BT_DBG("conn %p chan %p ident %d flags 0x%4.4x", conn, chan, ident,
4048 flags);
4049
4050 clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
4051 set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
4052
4053 l2cap_send_cmd(conn, ident, L2CAP_CONF_RSP,
4054 l2cap_build_conf_rsp(chan, data,
4055 L2CAP_CONF_SUCCESS, flags), data);
4056 }
4057
4058 static void cmd_reject_invalid_cid(struct l2cap_conn *conn, u8 ident,
4059 u16 scid, u16 dcid)
4060 {
4061 struct l2cap_cmd_rej_cid rej;
4062
4063 rej.reason = cpu_to_le16(L2CAP_REJ_INVALID_CID);
4064 rej.scid = __cpu_to_le16(scid);
4065 rej.dcid = __cpu_to_le16(dcid);
4066
4067 l2cap_send_cmd(conn, ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
4068 }
4069
4070 static inline int l2cap_config_req(struct l2cap_conn *conn,
4071 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4072 u8 *data)
4073 {
4074 struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
4075 u16 dcid, flags;
4076 u8 rsp[64];
4077 struct l2cap_chan *chan;
4078 int len, err = 0;
4079
4080 if (cmd_len < sizeof(*req))
4081 return -EPROTO;
4082
4083 dcid = __le16_to_cpu(req->dcid);
4084 flags = __le16_to_cpu(req->flags);
4085
4086 BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
4087
4088 chan = l2cap_get_chan_by_scid(conn, dcid);
4089 if (!chan) {
4090 cmd_reject_invalid_cid(conn, cmd->ident, dcid, 0);
4091 return 0;
4092 }
4093
4094 if (chan->state != BT_CONFIG && chan->state != BT_CONNECT2) {
4095 cmd_reject_invalid_cid(conn, cmd->ident, chan->scid,
4096 chan->dcid);
4097 goto unlock;
4098 }
4099
4100 /* Reject if config buffer is too small. */
4101 len = cmd_len - sizeof(*req);
4102 if (chan->conf_len + len > sizeof(chan->conf_req)) {
4103 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
4104 l2cap_build_conf_rsp(chan, rsp,
4105 L2CAP_CONF_REJECT, flags), rsp);
4106 goto unlock;
4107 }
4108
4109 /* Store config. */
4110 memcpy(chan->conf_req + chan->conf_len, req->data, len);
4111 chan->conf_len += len;
4112
4113 if (flags & L2CAP_CONF_FLAG_CONTINUATION) {
4114 /* Incomplete config. Send empty response. */
4115 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
4116 l2cap_build_conf_rsp(chan, rsp,
4117 L2CAP_CONF_SUCCESS, flags), rsp);
4118 goto unlock;
4119 }
4120
4121 /* Complete config. */
4122 len = l2cap_parse_conf_req(chan, rsp, sizeof(rsp));
4123 if (len < 0) {
4124 l2cap_send_disconn_req(chan, ECONNRESET);
4125 goto unlock;
4126 }
4127
4128 chan->ident = cmd->ident;
4129 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
4130 chan->num_conf_rsp++;
4131
4132 /* Reset config buffer. */
4133 chan->conf_len = 0;
4134
4135 if (!test_bit(CONF_OUTPUT_DONE, &chan->conf_state))
4136 goto unlock;
4137
4138 if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
4139 set_default_fcs(chan);
4140
4141 if (chan->mode == L2CAP_MODE_ERTM ||
4142 chan->mode == L2CAP_MODE_STREAMING)
4143 err = l2cap_ertm_init(chan);
4144
4145 if (err < 0)
4146 l2cap_send_disconn_req(chan, -err);
4147 else
4148 l2cap_chan_ready(chan);
4149
4150 goto unlock;
4151 }
4152
4153 if (!test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) {
4154 u8 buf[64];
4155 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
4156 l2cap_build_conf_req(chan, buf, sizeof(buf)), buf);
4157 chan->num_conf_req++;
4158 }
4159
4160 /* Got Conf Rsp PENDING from remote side and assume we sent
4161 Conf Rsp PENDING in the code above */
4162 if (test_bit(CONF_REM_CONF_PEND, &chan->conf_state) &&
4163 test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
4164
4165 /* check compatibility */
4166
4167 /* Send rsp for BR/EDR channel */
4168 if (!chan->hs_hcon)
4169 l2cap_send_efs_conf_rsp(chan, rsp, cmd->ident, flags);
4170 else
4171 chan->ident = cmd->ident;
4172 }
4173
4174 unlock:
4175 l2cap_chan_unlock(chan);
4176 return err;
4177 }
4178
4179 static inline int l2cap_config_rsp(struct l2cap_conn *conn,
4180 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4181 u8 *data)
4182 {
4183 struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
4184 u16 scid, flags, result;
4185 struct l2cap_chan *chan;
4186 int len = cmd_len - sizeof(*rsp);
4187 int err = 0;
4188
4189 if (cmd_len < sizeof(*rsp))
4190 return -EPROTO;
4191
4192 scid = __le16_to_cpu(rsp->scid);
4193 flags = __le16_to_cpu(rsp->flags);
4194 result = __le16_to_cpu(rsp->result);
4195
4196 BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x len %d", scid, flags,
4197 result, len);
4198
4199 chan = l2cap_get_chan_by_scid(conn, scid);
4200 if (!chan)
4201 return 0;
4202
4203 switch (result) {
4204 case L2CAP_CONF_SUCCESS:
4205 l2cap_conf_rfc_get(chan, rsp->data, len);
4206 clear_bit(CONF_REM_CONF_PEND, &chan->conf_state);
4207 break;
4208
4209 case L2CAP_CONF_PENDING:
4210 set_bit(CONF_REM_CONF_PEND, &chan->conf_state);
4211
4212 if (test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
4213 char buf[64];
4214
4215 len = l2cap_parse_conf_rsp(chan, rsp->data, len,
4216 buf, sizeof(buf), &result);
4217 if (len < 0) {
4218 l2cap_send_disconn_req(chan, ECONNRESET);
4219 goto done;
4220 }
4221
4222 if (!chan->hs_hcon) {
4223 l2cap_send_efs_conf_rsp(chan, buf, cmd->ident,
4224 0);
4225 } else {
4226 if (l2cap_check_efs(chan)) {
4227 amp_create_logical_link(chan);
4228 chan->ident = cmd->ident;
4229 }
4230 }
4231 }
4232 goto done;
4233
4234 case L2CAP_CONF_UNACCEPT:
4235 if (chan->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
4236 char req[64];
4237
4238 if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) {
4239 l2cap_send_disconn_req(chan, ECONNRESET);
4240 goto done;
4241 }
4242
4243 /* throw out any old stored conf requests */
4244 result = L2CAP_CONF_SUCCESS;
4245 len = l2cap_parse_conf_rsp(chan, rsp->data, len,
4246 req, sizeof(req), &result);
4247 if (len < 0) {
4248 l2cap_send_disconn_req(chan, ECONNRESET);
4249 goto done;
4250 }
4251
4252 l2cap_send_cmd(conn, l2cap_get_ident(conn),
4253 L2CAP_CONF_REQ, len, req);
4254 chan->num_conf_req++;
4255 if (result != L2CAP_CONF_SUCCESS)
4256 goto done;
4257 break;
4258 }
4259
4260 default:
4261 l2cap_chan_set_err(chan, ECONNRESET);
4262
4263 __set_chan_timer(chan, L2CAP_DISC_REJ_TIMEOUT);
4264 l2cap_send_disconn_req(chan, ECONNRESET);
4265 goto done;
4266 }
4267
4268 if (flags & L2CAP_CONF_FLAG_CONTINUATION)
4269 goto done;
4270
4271 set_bit(CONF_INPUT_DONE, &chan->conf_state);
4272
4273 if (test_bit(CONF_OUTPUT_DONE, &chan->conf_state)) {
4274 set_default_fcs(chan);
4275
4276 if (chan->mode == L2CAP_MODE_ERTM ||
4277 chan->mode == L2CAP_MODE_STREAMING)
4278 err = l2cap_ertm_init(chan);
4279
4280 if (err < 0)
4281 l2cap_send_disconn_req(chan, -err);
4282 else
4283 l2cap_chan_ready(chan);
4284 }
4285
4286 done:
4287 l2cap_chan_unlock(chan);
4288 return err;
4289 }
4290
4291 static inline int l2cap_disconnect_req(struct l2cap_conn *conn,
4292 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4293 u8 *data)
4294 {
4295 struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
4296 struct l2cap_disconn_rsp rsp;
4297 u16 dcid, scid;
4298 struct l2cap_chan *chan;
4299
4300 if (cmd_len != sizeof(*req))
4301 return -EPROTO;
4302
4303 scid = __le16_to_cpu(req->scid);
4304 dcid = __le16_to_cpu(req->dcid);
4305
4306 BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
4307
4308 mutex_lock(&conn->chan_lock);
4309
4310 chan = __l2cap_get_chan_by_scid(conn, dcid);
4311 if (!chan) {
4312 mutex_unlock(&conn->chan_lock);
4313 cmd_reject_invalid_cid(conn, cmd->ident, dcid, scid);
4314 return 0;
4315 }
4316
4317 l2cap_chan_lock(chan);
4318
4319 rsp.dcid = cpu_to_le16(chan->scid);
4320 rsp.scid = cpu_to_le16(chan->dcid);
4321 l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
4322
4323 chan->ops->set_shutdown(chan);
4324
4325 l2cap_chan_hold(chan);
4326 l2cap_chan_del(chan, ECONNRESET);
4327
4328 l2cap_chan_unlock(chan);
4329
4330 chan->ops->close(chan);
4331 l2cap_chan_put(chan);
4332
4333 mutex_unlock(&conn->chan_lock);
4334
4335 return 0;
4336 }
4337
4338 static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn,
4339 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4340 u8 *data)
4341 {
4342 struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
4343 u16 dcid, scid;
4344 struct l2cap_chan *chan;
4345
4346 if (cmd_len != sizeof(*rsp))
4347 return -EPROTO;
4348
4349 scid = __le16_to_cpu(rsp->scid);
4350 dcid = __le16_to_cpu(rsp->dcid);
4351
4352 BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
4353
4354 mutex_lock(&conn->chan_lock);
4355
4356 chan = __l2cap_get_chan_by_scid(conn, scid);
4357 if (!chan) {
4358 mutex_unlock(&conn->chan_lock);
4359 return 0;
4360 }
4361
4362 l2cap_chan_lock(chan);
4363
4364 l2cap_chan_hold(chan);
4365 l2cap_chan_del(chan, 0);
4366
4367 l2cap_chan_unlock(chan);
4368
4369 chan->ops->close(chan);
4370 l2cap_chan_put(chan);
4371
4372 mutex_unlock(&conn->chan_lock);
4373
4374 return 0;
4375 }
4376
4377 static inline int l2cap_information_req(struct l2cap_conn *conn,
4378 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4379 u8 *data)
4380 {
4381 struct l2cap_info_req *req = (struct l2cap_info_req *) data;
4382 u16 type;
4383
4384 if (cmd_len != sizeof(*req))
4385 return -EPROTO;
4386
4387 type = __le16_to_cpu(req->type);
4388
4389 BT_DBG("type 0x%4.4x", type);
4390
4391 if (type == L2CAP_IT_FEAT_MASK) {
4392 u8 buf[8];
4393 u32 feat_mask = l2cap_feat_mask;
4394 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
4395 rsp->type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
4396 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
4397 if (!disable_ertm)
4398 feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
4399 | L2CAP_FEAT_FCS;
4400 if (conn->local_fixed_chan & L2CAP_FC_A2MP)
4401 feat_mask |= L2CAP_FEAT_EXT_FLOW
4402 | L2CAP_FEAT_EXT_WINDOW;
4403
4404 put_unaligned_le32(feat_mask, rsp->data);
4405 l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf),
4406 buf);
4407 } else if (type == L2CAP_IT_FIXED_CHAN) {
4408 u8 buf[12];
4409 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
4410
4411 rsp->type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
4412 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
4413 rsp->data[0] = conn->local_fixed_chan;
4414 memset(rsp->data + 1, 0, 7);
4415 l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf),
4416 buf);
4417 } else {
4418 struct l2cap_info_rsp rsp;
4419 rsp.type = cpu_to_le16(type);
4420 rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP);
4421 l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(rsp),
4422 &rsp);
4423 }
4424
4425 return 0;
4426 }
4427
4428 static inline int l2cap_information_rsp(struct l2cap_conn *conn,
4429 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4430 u8 *data)
4431 {
4432 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
4433 u16 type, result;
4434
4435 if (cmd_len < sizeof(*rsp))
4436 return -EPROTO;
4437
4438 type = __le16_to_cpu(rsp->type);
4439 result = __le16_to_cpu(rsp->result);
4440
4441 BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
4442
4443 /* L2CAP Info req/rsp are unbound to channels, add extra checks */
4444 if (cmd->ident != conn->info_ident ||
4445 conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)
4446 return 0;
4447
4448 cancel_delayed_work(&conn->info_timer);
4449
4450 if (result != L2CAP_IR_SUCCESS) {
4451 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4452 conn->info_ident = 0;
4453
4454 l2cap_conn_start(conn);
4455
4456 return 0;
4457 }
4458
4459 switch (type) {
4460 case L2CAP_IT_FEAT_MASK:
4461 conn->feat_mask = get_unaligned_le32(rsp->data);
4462
4463 if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
4464 struct l2cap_info_req req;
4465 req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
4466
4467 conn->info_ident = l2cap_get_ident(conn);
4468
4469 l2cap_send_cmd(conn, conn->info_ident,
4470 L2CAP_INFO_REQ, sizeof(req), &req);
4471 } else {
4472 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4473 conn->info_ident = 0;
4474
4475 l2cap_conn_start(conn);
4476 }
4477 break;
4478
4479 case L2CAP_IT_FIXED_CHAN:
4480 conn->remote_fixed_chan = rsp->data[0];
4481 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4482 conn->info_ident = 0;
4483
4484 l2cap_conn_start(conn);
4485 break;
4486 }
4487
4488 return 0;
4489 }
4490
4491 static int l2cap_create_channel_req(struct l2cap_conn *conn,
4492 struct l2cap_cmd_hdr *cmd,
4493 u16 cmd_len, void *data)
4494 {
4495 struct l2cap_create_chan_req *req = data;
4496 struct l2cap_create_chan_rsp rsp;
4497 struct l2cap_chan *chan;
4498 struct hci_dev *hdev;
4499 u16 psm, scid;
4500
4501 if (cmd_len != sizeof(*req))
4502 return -EPROTO;
4503
4504 if (!(conn->local_fixed_chan & L2CAP_FC_A2MP))
4505 return -EINVAL;
4506
4507 psm = le16_to_cpu(req->psm);
4508 scid = le16_to_cpu(req->scid);
4509
4510 BT_DBG("psm 0x%2.2x, scid 0x%4.4x, amp_id %d", psm, scid, req->amp_id);
4511
4512 /* For controller id 0 make BR/EDR connection */
4513 if (req->amp_id == AMP_ID_BREDR) {
4514 l2cap_connect(conn, cmd, data, L2CAP_CREATE_CHAN_RSP,
4515 req->amp_id);
4516 return 0;
4517 }
4518
4519 /* Validate AMP controller id */
4520 hdev = hci_dev_get(req->amp_id);
4521 if (!hdev)
4522 goto error;
4523
4524 if (hdev->dev_type != HCI_AMP || !test_bit(HCI_UP, &hdev->flags)) {
4525 hci_dev_put(hdev);
4526 goto error;
4527 }
4528
4529 chan = l2cap_connect(conn, cmd, data, L2CAP_CREATE_CHAN_RSP,
4530 req->amp_id);
4531 if (chan) {
4532 struct amp_mgr *mgr = conn->hcon->amp_mgr;
4533 struct hci_conn *hs_hcon;
4534
4535 hs_hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK,
4536 &conn->hcon->dst);
4537 if (!hs_hcon) {
4538 hci_dev_put(hdev);
4539 cmd_reject_invalid_cid(conn, cmd->ident, chan->scid,
4540 chan->dcid);
4541 return 0;
4542 }
4543
4544 BT_DBG("mgr %p bredr_chan %p hs_hcon %p", mgr, chan, hs_hcon);
4545
4546 mgr->bredr_chan = chan;
4547 chan->hs_hcon = hs_hcon;
4548 chan->fcs = L2CAP_FCS_NONE;
4549 conn->mtu = hdev->block_mtu;
4550 }
4551
4552 hci_dev_put(hdev);
4553
4554 return 0;
4555
4556 error:
4557 rsp.dcid = 0;
4558 rsp.scid = cpu_to_le16(scid);
4559 rsp.result = cpu_to_le16(L2CAP_CR_BAD_AMP);
4560 rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
4561
4562 l2cap_send_cmd(conn, cmd->ident, L2CAP_CREATE_CHAN_RSP,
4563 sizeof(rsp), &rsp);
4564
4565 return 0;
4566 }
4567
4568 static void l2cap_send_move_chan_req(struct l2cap_chan *chan, u8 dest_amp_id)
4569 {
4570 struct l2cap_move_chan_req req;
4571 u8 ident;
4572
4573 BT_DBG("chan %p, dest_amp_id %d", chan, dest_amp_id);
4574
4575 ident = l2cap_get_ident(chan->conn);
4576 chan->ident = ident;
4577
4578 req.icid = cpu_to_le16(chan->scid);
4579 req.dest_amp_id = dest_amp_id;
4580
4581 l2cap_send_cmd(chan->conn, ident, L2CAP_MOVE_CHAN_REQ, sizeof(req),
4582 &req);
4583
4584 __set_chan_timer(chan, L2CAP_MOVE_TIMEOUT);
4585 }
4586
4587 static void l2cap_send_move_chan_rsp(struct l2cap_chan *chan, u16 result)
4588 {
4589 struct l2cap_move_chan_rsp rsp;
4590
4591 BT_DBG("chan %p, result 0x%4.4x", chan, result);
4592
4593 rsp.icid = cpu_to_le16(chan->dcid);
4594 rsp.result = cpu_to_le16(result);
4595
4596 l2cap_send_cmd(chan->conn, chan->ident, L2CAP_MOVE_CHAN_RSP,
4597 sizeof(rsp), &rsp);
4598 }
4599
4600 static void l2cap_send_move_chan_cfm(struct l2cap_chan *chan, u16 result)
4601 {
4602 struct l2cap_move_chan_cfm cfm;
4603
4604 BT_DBG("chan %p, result 0x%4.4x", chan, result);
4605
4606 chan->ident = l2cap_get_ident(chan->conn);
4607
4608 cfm.icid = cpu_to_le16(chan->scid);
4609 cfm.result = cpu_to_le16(result);
4610
4611 l2cap_send_cmd(chan->conn, chan->ident, L2CAP_MOVE_CHAN_CFM,
4612 sizeof(cfm), &cfm);
4613
4614 __set_chan_timer(chan, L2CAP_MOVE_TIMEOUT);
4615 }
4616
4617 static void l2cap_send_move_chan_cfm_icid(struct l2cap_conn *conn, u16 icid)
4618 {
4619 struct l2cap_move_chan_cfm cfm;
4620
4621 BT_DBG("conn %p, icid 0x%4.4x", conn, icid);
4622
4623 cfm.icid = cpu_to_le16(icid);
4624 cfm.result = cpu_to_le16(L2CAP_MC_UNCONFIRMED);
4625
4626 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_MOVE_CHAN_CFM,
4627 sizeof(cfm), &cfm);
4628 }
4629
4630 static void l2cap_send_move_chan_cfm_rsp(struct l2cap_conn *conn, u8 ident,
4631 u16 icid)
4632 {
4633 struct l2cap_move_chan_cfm_rsp rsp;
4634
4635 BT_DBG("icid 0x%4.4x", icid);
4636
4637 rsp.icid = cpu_to_le16(icid);
4638 l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM_RSP, sizeof(rsp), &rsp);
4639 }
4640
4641 static void __release_logical_link(struct l2cap_chan *chan)
4642 {
4643 chan->hs_hchan = NULL;
4644 chan->hs_hcon = NULL;
4645
4646 /* Placeholder - release the logical link */
4647 }
4648
4649 static void l2cap_logical_fail(struct l2cap_chan *chan)
4650 {
4651 /* Logical link setup failed */
4652 if (chan->state != BT_CONNECTED) {
4653 /* Create channel failure, disconnect */
4654 l2cap_send_disconn_req(chan, ECONNRESET);
4655 return;
4656 }
4657
4658 switch (chan->move_role) {
4659 case L2CAP_MOVE_ROLE_RESPONDER:
4660 l2cap_move_done(chan);
4661 l2cap_send_move_chan_rsp(chan, L2CAP_MR_NOT_SUPP);
4662 break;
4663 case L2CAP_MOVE_ROLE_INITIATOR:
4664 if (chan->move_state == L2CAP_MOVE_WAIT_LOGICAL_COMP ||
4665 chan->move_state == L2CAP_MOVE_WAIT_LOGICAL_CFM) {
4666 /* Remote has only sent pending or
4667 * success responses, clean up
4668 */
4669 l2cap_move_done(chan);
4670 }
4671
4672 /* Other amp move states imply that the move
4673 * has already aborted
4674 */
4675 l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
4676 break;
4677 }
4678 }
4679
4680 static void l2cap_logical_finish_create(struct l2cap_chan *chan,
4681 struct hci_chan *hchan)
4682 {
4683 struct l2cap_conf_rsp rsp;
4684
4685 chan->hs_hchan = hchan;
4686 chan->hs_hcon->l2cap_data = chan->conn;
4687
4688 l2cap_send_efs_conf_rsp(chan, &rsp, chan->ident, 0);
4689
4690 if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
4691 int err;
4692
4693 set_default_fcs(chan);
4694
4695 err = l2cap_ertm_init(chan);
4696 if (err < 0)
4697 l2cap_send_disconn_req(chan, -err);
4698 else
4699 l2cap_chan_ready(chan);
4700 }
4701 }
4702
4703 static void l2cap_logical_finish_move(struct l2cap_chan *chan,
4704 struct hci_chan *hchan)
4705 {
4706 chan->hs_hcon = hchan->conn;
4707 chan->hs_hcon->l2cap_data = chan->conn;
4708
4709 BT_DBG("move_state %d", chan->move_state);
4710
4711 switch (chan->move_state) {
4712 case L2CAP_MOVE_WAIT_LOGICAL_COMP:
4713 /* Move confirm will be sent after a success
4714 * response is received
4715 */
4716 chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
4717 break;
4718 case L2CAP_MOVE_WAIT_LOGICAL_CFM:
4719 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
4720 chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
4721 } else if (chan->move_role == L2CAP_MOVE_ROLE_INITIATOR) {
4722 chan->move_state = L2CAP_MOVE_WAIT_CONFIRM_RSP;
4723 l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
4724 } else if (chan->move_role == L2CAP_MOVE_ROLE_RESPONDER) {
4725 chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
4726 l2cap_send_move_chan_rsp(chan, L2CAP_MR_SUCCESS);
4727 }
4728 break;
4729 default:
4730 /* Move was not in expected state, free the channel */
4731 __release_logical_link(chan);
4732
4733 chan->move_state = L2CAP_MOVE_STABLE;
4734 }
4735 }
4736
4737 /* Call with chan locked */
4738 void l2cap_logical_cfm(struct l2cap_chan *chan, struct hci_chan *hchan,
4739 u8 status)
4740 {
4741 BT_DBG("chan %p, hchan %p, status %d", chan, hchan, status);
4742
4743 if (status) {
4744 l2cap_logical_fail(chan);
4745 __release_logical_link(chan);
4746 return;
4747 }
4748
4749 if (chan->state != BT_CONNECTED) {
4750 /* Ignore logical link if channel is on BR/EDR */
4751 if (chan->local_amp_id != AMP_ID_BREDR)
4752 l2cap_logical_finish_create(chan, hchan);
4753 } else {
4754 l2cap_logical_finish_move(chan, hchan);
4755 }
4756 }
4757
4758 void l2cap_move_start(struct l2cap_chan *chan)
4759 {
4760 BT_DBG("chan %p", chan);
4761
4762 if (chan->local_amp_id == AMP_ID_BREDR) {
4763 if (chan->chan_policy != BT_CHANNEL_POLICY_AMP_PREFERRED)
4764 return;
4765 chan->move_role = L2CAP_MOVE_ROLE_INITIATOR;
4766 chan->move_state = L2CAP_MOVE_WAIT_PREPARE;
4767 /* Placeholder - start physical link setup */
4768 } else {
4769 chan->move_role = L2CAP_MOVE_ROLE_INITIATOR;
4770 chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
4771 chan->move_id = 0;
4772 l2cap_move_setup(chan);
4773 l2cap_send_move_chan_req(chan, 0);
4774 }
4775 }
4776
4777 static void l2cap_do_create(struct l2cap_chan *chan, int result,
4778 u8 local_amp_id, u8 remote_amp_id)
4779 {
4780 BT_DBG("chan %p state %s %u -> %u", chan, state_to_string(chan->state),
4781 local_amp_id, remote_amp_id);
4782
4783 chan->fcs = L2CAP_FCS_NONE;
4784
4785 /* Outgoing channel on AMP */
4786 if (chan->state == BT_CONNECT) {
4787 if (result == L2CAP_CR_SUCCESS) {
4788 chan->local_amp_id = local_amp_id;
4789 l2cap_send_create_chan_req(chan, remote_amp_id);
4790 } else {
4791 /* Revert to BR/EDR connect */
4792 l2cap_send_conn_req(chan);
4793 }
4794
4795 return;
4796 }
4797
4798 /* Incoming channel on AMP */
4799 if (__l2cap_no_conn_pending(chan)) {
4800 struct l2cap_conn_rsp rsp;
4801 char buf[128];
4802 rsp.scid = cpu_to_le16(chan->dcid);
4803 rsp.dcid = cpu_to_le16(chan->scid);
4804
4805 if (result == L2CAP_CR_SUCCESS) {
4806 /* Send successful response */
4807 rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
4808 rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
4809 } else {
4810 /* Send negative response */
4811 rsp.result = cpu_to_le16(L2CAP_CR_NO_MEM);
4812 rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
4813 }
4814
4815 l2cap_send_cmd(chan->conn, chan->ident, L2CAP_CREATE_CHAN_RSP,
4816 sizeof(rsp), &rsp);
4817
4818 if (result == L2CAP_CR_SUCCESS) {
4819 l2cap_state_change(chan, BT_CONFIG);
4820 set_bit(CONF_REQ_SENT, &chan->conf_state);
4821 l2cap_send_cmd(chan->conn, l2cap_get_ident(chan->conn),
4822 L2CAP_CONF_REQ,
4823 l2cap_build_conf_req(chan, buf, sizeof(buf)), buf);
4824 chan->num_conf_req++;
4825 }
4826 }
4827 }
4828
4829 static void l2cap_do_move_initiate(struct l2cap_chan *chan, u8 local_amp_id,
4830 u8 remote_amp_id)
4831 {
4832 l2cap_move_setup(chan);
4833 chan->move_id = local_amp_id;
4834 chan->move_state = L2CAP_MOVE_WAIT_RSP;
4835
4836 l2cap_send_move_chan_req(chan, remote_amp_id);
4837 }
4838
4839 static void l2cap_do_move_respond(struct l2cap_chan *chan, int result)
4840 {
4841 struct hci_chan *hchan = NULL;
4842
4843 /* Placeholder - get hci_chan for logical link */
4844
4845 if (hchan) {
4846 if (hchan->state == BT_CONNECTED) {
4847 /* Logical link is ready to go */
4848 chan->hs_hcon = hchan->conn;
4849 chan->hs_hcon->l2cap_data = chan->conn;
4850 chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
4851 l2cap_send_move_chan_rsp(chan, L2CAP_MR_SUCCESS);
4852
4853 l2cap_logical_cfm(chan, hchan, L2CAP_MR_SUCCESS);
4854 } else {
4855 /* Wait for logical link to be ready */
4856 chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
4857 }
4858 } else {
4859 /* Logical link not available */
4860 l2cap_send_move_chan_rsp(chan, L2CAP_MR_NOT_ALLOWED);
4861 }
4862 }
4863
4864 static void l2cap_do_move_cancel(struct l2cap_chan *chan, int result)
4865 {
4866 if (chan->move_role == L2CAP_MOVE_ROLE_RESPONDER) {
4867 u8 rsp_result;
4868 if (result == -EINVAL)
4869 rsp_result = L2CAP_MR_BAD_ID;
4870 else
4871 rsp_result = L2CAP_MR_NOT_ALLOWED;
4872
4873 l2cap_send_move_chan_rsp(chan, rsp_result);
4874 }
4875
4876 chan->move_role = L2CAP_MOVE_ROLE_NONE;
4877 chan->move_state = L2CAP_MOVE_STABLE;
4878
4879 /* Restart data transmission */
4880 l2cap_ertm_send(chan);
4881 }
4882
4883 /* Invoke with locked chan */
4884 void __l2cap_physical_cfm(struct l2cap_chan *chan, int result)
4885 {
4886 u8 local_amp_id = chan->local_amp_id;
4887 u8 remote_amp_id = chan->remote_amp_id;
4888
4889 BT_DBG("chan %p, result %d, local_amp_id %d, remote_amp_id %d",
4890 chan, result, local_amp_id, remote_amp_id);
4891
4892 if (chan->state == BT_DISCONN || chan->state == BT_CLOSED) {
4893 l2cap_chan_unlock(chan);
4894 return;
4895 }
4896
4897 if (chan->state != BT_CONNECTED) {
4898 l2cap_do_create(chan, result, local_amp_id, remote_amp_id);
4899 } else if (result != L2CAP_MR_SUCCESS) {
4900 l2cap_do_move_cancel(chan, result);
4901 } else {
4902 switch (chan->move_role) {
4903 case L2CAP_MOVE_ROLE_INITIATOR:
4904 l2cap_do_move_initiate(chan, local_amp_id,
4905 remote_amp_id);
4906 break;
4907 case L2CAP_MOVE_ROLE_RESPONDER:
4908 l2cap_do_move_respond(chan, result);
4909 break;
4910 default:
4911 l2cap_do_move_cancel(chan, result);
4912 break;
4913 }
4914 }
4915 }
4916
4917 static inline int l2cap_move_channel_req(struct l2cap_conn *conn,
4918 struct l2cap_cmd_hdr *cmd,
4919 u16 cmd_len, void *data)
4920 {
4921 struct l2cap_move_chan_req *req = data;
4922 struct l2cap_move_chan_rsp rsp;
4923 struct l2cap_chan *chan;
4924 u16 icid = 0;
4925 u16 result = L2CAP_MR_NOT_ALLOWED;
4926
4927 if (cmd_len != sizeof(*req))
4928 return -EPROTO;
4929
4930 icid = le16_to_cpu(req->icid);
4931
4932 BT_DBG("icid 0x%4.4x, dest_amp_id %d", icid, req->dest_amp_id);
4933
4934 if (!(conn->local_fixed_chan & L2CAP_FC_A2MP))
4935 return -EINVAL;
4936
4937 chan = l2cap_get_chan_by_dcid(conn, icid);
4938 if (!chan) {
4939 rsp.icid = cpu_to_le16(icid);
4940 rsp.result = cpu_to_le16(L2CAP_MR_NOT_ALLOWED);
4941 l2cap_send_cmd(conn, cmd->ident, L2CAP_MOVE_CHAN_RSP,
4942 sizeof(rsp), &rsp);
4943 return 0;
4944 }
4945
4946 chan->ident = cmd->ident;
4947
4948 if (chan->scid < L2CAP_CID_DYN_START ||
4949 chan->chan_policy == BT_CHANNEL_POLICY_BREDR_ONLY ||
4950 (chan->mode != L2CAP_MODE_ERTM &&
4951 chan->mode != L2CAP_MODE_STREAMING)) {
4952 result = L2CAP_MR_NOT_ALLOWED;
4953 goto send_move_response;
4954 }
4955
4956 if (chan->local_amp_id == req->dest_amp_id) {
4957 result = L2CAP_MR_SAME_ID;
4958 goto send_move_response;
4959 }
4960
4961 if (req->dest_amp_id != AMP_ID_BREDR) {
4962 struct hci_dev *hdev;
4963 hdev = hci_dev_get(req->dest_amp_id);
4964 if (!hdev || hdev->dev_type != HCI_AMP ||
4965 !test_bit(HCI_UP, &hdev->flags)) {
4966 if (hdev)
4967 hci_dev_put(hdev);
4968
4969 result = L2CAP_MR_BAD_ID;
4970 goto send_move_response;
4971 }
4972 hci_dev_put(hdev);
4973 }
4974
4975 /* Detect a move collision. Only send a collision response
4976 * if this side has "lost", otherwise proceed with the move.
4977 * The winner has the larger bd_addr.
4978 */
4979 if ((__chan_is_moving(chan) ||
4980 chan->move_role != L2CAP_MOVE_ROLE_NONE) &&
4981 bacmp(&conn->hcon->src, &conn->hcon->dst) > 0) {
4982 result = L2CAP_MR_COLLISION;
4983 goto send_move_response;
4984 }
4985
4986 chan->move_role = L2CAP_MOVE_ROLE_RESPONDER;
4987 l2cap_move_setup(chan);
4988 chan->move_id = req->dest_amp_id;
4989 icid = chan->dcid;
4990
4991 if (req->dest_amp_id == AMP_ID_BREDR) {
4992 /* Moving to BR/EDR */
4993 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
4994 chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
4995 result = L2CAP_MR_PEND;
4996 } else {
4997 chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
4998 result = L2CAP_MR_SUCCESS;
4999 }
5000 } else {
5001 chan->move_state = L2CAP_MOVE_WAIT_PREPARE;
5002 /* Placeholder - uncomment when amp functions are available */
5003 /*amp_accept_physical(chan, req->dest_amp_id);*/
5004 result = L2CAP_MR_PEND;
5005 }
5006
5007 send_move_response:
5008 l2cap_send_move_chan_rsp(chan, result);
5009
5010 l2cap_chan_unlock(chan);
5011
5012 return 0;
5013 }
5014
5015 static void l2cap_move_continue(struct l2cap_conn *conn, u16 icid, u16 result)
5016 {
5017 struct l2cap_chan *chan;
5018 struct hci_chan *hchan = NULL;
5019
5020 chan = l2cap_get_chan_by_scid(conn, icid);
5021 if (!chan) {
5022 l2cap_send_move_chan_cfm_icid(conn, icid);
5023 return;
5024 }
5025
5026 __clear_chan_timer(chan);
5027 if (result == L2CAP_MR_PEND)
5028 __set_chan_timer(chan, L2CAP_MOVE_ERTX_TIMEOUT);
5029
5030 switch (chan->move_state) {
5031 case L2CAP_MOVE_WAIT_LOGICAL_COMP:
5032 /* Move confirm will be sent when logical link
5033 * is complete.
5034 */
5035 chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
5036 break;
5037 case L2CAP_MOVE_WAIT_RSP_SUCCESS:
5038 if (result == L2CAP_MR_PEND) {
5039 break;
5040 } else if (test_bit(CONN_LOCAL_BUSY,
5041 &chan->conn_state)) {
5042 chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
5043 } else {
5044 /* Logical link is up or moving to BR/EDR,
5045 * proceed with move
5046 */
5047 chan->move_state = L2CAP_MOVE_WAIT_CONFIRM_RSP;
5048 l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
5049 }
5050 break;
5051 case L2CAP_MOVE_WAIT_RSP:
5052 /* Moving to AMP */
5053 if (result == L2CAP_MR_SUCCESS) {
5054 /* Remote is ready, send confirm immediately
5055 * after logical link is ready
5056 */
5057 chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
5058 } else {
5059 /* Both logical link and move success
5060 * are required to confirm
5061 */
5062 chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_COMP;
5063 }
5064
5065 /* Placeholder - get hci_chan for logical link */
5066 if (!hchan) {
5067 /* Logical link not available */
5068 l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
5069 break;
5070 }
5071
5072 /* If the logical link is not yet connected, do not
5073 * send confirmation.
5074 */
5075 if (hchan->state != BT_CONNECTED)
5076 break;
5077
5078 /* Logical link is already ready to go */
5079
5080 chan->hs_hcon = hchan->conn;
5081 chan->hs_hcon->l2cap_data = chan->conn;
5082
5083 if (result == L2CAP_MR_SUCCESS) {
5084 /* Can confirm now */
5085 l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
5086 } else {
5087 /* Now only need move success
5088 * to confirm
5089 */
5090 chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
5091 }
5092
5093 l2cap_logical_cfm(chan, hchan, L2CAP_MR_SUCCESS);
5094 break;
5095 default:
5096 /* Any other amp move state means the move failed. */
5097 chan->move_id = chan->local_amp_id;
5098 l2cap_move_done(chan);
5099 l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
5100 }
5101
5102 l2cap_chan_unlock(chan);
5103 }
5104
5105 static void l2cap_move_fail(struct l2cap_conn *conn, u8 ident, u16 icid,
5106 u16 result)
5107 {
5108 struct l2cap_chan *chan;
5109
5110 chan = l2cap_get_chan_by_ident(conn, ident);
5111 if (!chan) {
5112 /* Could not locate channel, icid is best guess */
5113 l2cap_send_move_chan_cfm_icid(conn, icid);
5114 return;
5115 }
5116
5117 __clear_chan_timer(chan);
5118
5119 if (chan->move_role == L2CAP_MOVE_ROLE_INITIATOR) {
5120 if (result == L2CAP_MR_COLLISION) {
5121 chan->move_role = L2CAP_MOVE_ROLE_RESPONDER;
5122 } else {
5123 /* Cleanup - cancel move */
5124 chan->move_id = chan->local_amp_id;
5125 l2cap_move_done(chan);
5126 }
5127 }
5128
5129 l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
5130
5131 l2cap_chan_unlock(chan);
5132 }
5133
5134 static int l2cap_move_channel_rsp(struct l2cap_conn *conn,
5135 struct l2cap_cmd_hdr *cmd,
5136 u16 cmd_len, void *data)
5137 {
5138 struct l2cap_move_chan_rsp *rsp = data;
5139 u16 icid, result;
5140
5141 if (cmd_len != sizeof(*rsp))
5142 return -EPROTO;
5143
5144 icid = le16_to_cpu(rsp->icid);
5145 result = le16_to_cpu(rsp->result);
5146
5147 BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result);
5148
5149 if (result == L2CAP_MR_SUCCESS || result == L2CAP_MR_PEND)
5150 l2cap_move_continue(conn, icid, result);
5151 else
5152 l2cap_move_fail(conn, cmd->ident, icid, result);
5153
5154 return 0;
5155 }
5156
5157 static int l2cap_move_channel_confirm(struct l2cap_conn *conn,
5158 struct l2cap_cmd_hdr *cmd,
5159 u16 cmd_len, void *data)
5160 {
5161 struct l2cap_move_chan_cfm *cfm = data;
5162 struct l2cap_chan *chan;
5163 u16 icid, result;
5164
5165 if (cmd_len != sizeof(*cfm))
5166 return -EPROTO;
5167
5168 icid = le16_to_cpu(cfm->icid);
5169 result = le16_to_cpu(cfm->result);
5170
5171 BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result);
5172
5173 chan = l2cap_get_chan_by_dcid(conn, icid);
5174 if (!chan) {
5175 /* Spec requires a response even if the icid was not found */
5176 l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid);
5177 return 0;
5178 }
5179
5180 if (chan->move_state == L2CAP_MOVE_WAIT_CONFIRM) {
5181 if (result == L2CAP_MC_CONFIRMED) {
5182 chan->local_amp_id = chan->move_id;
5183 if (chan->local_amp_id == AMP_ID_BREDR)
5184 __release_logical_link(chan);
5185 } else {
5186 chan->move_id = chan->local_amp_id;
5187 }
5188
5189 l2cap_move_done(chan);
5190 }
5191
5192 l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid);
5193
5194 l2cap_chan_unlock(chan);
5195
5196 return 0;
5197 }
5198
5199 static inline int l2cap_move_channel_confirm_rsp(struct l2cap_conn *conn,
5200 struct l2cap_cmd_hdr *cmd,
5201 u16 cmd_len, void *data)
5202 {
5203 struct l2cap_move_chan_cfm_rsp *rsp = data;
5204 struct l2cap_chan *chan;
5205 u16 icid;
5206
5207 if (cmd_len != sizeof(*rsp))
5208 return -EPROTO;
5209
5210 icid = le16_to_cpu(rsp->icid);
5211
5212 BT_DBG("icid 0x%4.4x", icid);
5213
5214 chan = l2cap_get_chan_by_scid(conn, icid);
5215 if (!chan)
5216 return 0;
5217
5218 __clear_chan_timer(chan);
5219
5220 if (chan->move_state == L2CAP_MOVE_WAIT_CONFIRM_RSP) {
5221 chan->local_amp_id = chan->move_id;
5222
5223 if (chan->local_amp_id == AMP_ID_BREDR && chan->hs_hchan)
5224 __release_logical_link(chan);
5225
5226 l2cap_move_done(chan);
5227 }
5228
5229 l2cap_chan_unlock(chan);
5230
5231 return 0;
5232 }
5233
5234 static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
5235 struct l2cap_cmd_hdr *cmd,
5236 u16 cmd_len, u8 *data)
5237 {
5238 struct hci_conn *hcon = conn->hcon;
5239 struct l2cap_conn_param_update_req *req;
5240 struct l2cap_conn_param_update_rsp rsp;
5241 u16 min, max, latency, to_multiplier;
5242 int err;
5243
5244 if (hcon->role != HCI_ROLE_MASTER)
5245 return -EINVAL;
5246
5247 if (cmd_len != sizeof(struct l2cap_conn_param_update_req))
5248 return -EPROTO;
5249
5250 req = (struct l2cap_conn_param_update_req *) data;
5251 min = __le16_to_cpu(req->min);
5252 max = __le16_to_cpu(req->max);
5253 latency = __le16_to_cpu(req->latency);
5254 to_multiplier = __le16_to_cpu(req->to_multiplier);
5255
5256 BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x",
5257 min, max, latency, to_multiplier);
5258
5259 memset(&rsp, 0, sizeof(rsp));
5260
5261 err = hci_check_conn_params(min, max, latency, to_multiplier);
5262 if (err)
5263 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_REJECTED);
5264 else
5265 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED);
5266
5267 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP,
5268 sizeof(rsp), &rsp);
5269
5270 if (!err) {
5271 u8 store_hint;
5272
5273 store_hint = hci_le_conn_update(hcon, min, max, latency,
5274 to_multiplier);
5275 mgmt_new_conn_param(hcon->hdev, &hcon->dst, hcon->dst_type,
5276 store_hint, min, max, latency,
5277 to_multiplier);
5278
5279 }
5280
5281 return 0;
5282 }
5283
5284 static int l2cap_le_connect_rsp(struct l2cap_conn *conn,
5285 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5286 u8 *data)
5287 {
5288 struct l2cap_le_conn_rsp *rsp = (struct l2cap_le_conn_rsp *) data;
5289 struct hci_conn *hcon = conn->hcon;
5290 u16 dcid, mtu, mps, credits, result;
5291 struct l2cap_chan *chan;
5292 int err, sec_level;
5293
5294 if (cmd_len < sizeof(*rsp))
5295 return -EPROTO;
5296
5297 dcid = __le16_to_cpu(rsp->dcid);
5298 mtu = __le16_to_cpu(rsp->mtu);
5299 mps = __le16_to_cpu(rsp->mps);
5300 credits = __le16_to_cpu(rsp->credits);
5301 result = __le16_to_cpu(rsp->result);
5302
5303 if (result == L2CAP_CR_SUCCESS && (mtu < 23 || mps < 23 ||
5304 dcid < L2CAP_CID_DYN_START ||
5305 dcid > L2CAP_CID_LE_DYN_END))
5306 return -EPROTO;
5307
5308 BT_DBG("dcid 0x%4.4x mtu %u mps %u credits %u result 0x%2.2x",
5309 dcid, mtu, mps, credits, result);
5310
5311 mutex_lock(&conn->chan_lock);
5312
5313 chan = __l2cap_get_chan_by_ident(conn, cmd->ident);
5314 if (!chan) {
5315 err = -EBADSLT;
5316 goto unlock;
5317 }
5318
5319 err = 0;
5320
5321 l2cap_chan_lock(chan);
5322
5323 switch (result) {
5324 case L2CAP_CR_SUCCESS:
5325 if (__l2cap_get_chan_by_dcid(conn, dcid)) {
5326 err = -EBADSLT;
5327 break;
5328 }
5329
5330 chan->ident = 0;
5331 chan->dcid = dcid;
5332 chan->omtu = mtu;
5333 chan->remote_mps = mps;
5334 chan->tx_credits = credits;
5335 l2cap_chan_ready(chan);
5336 break;
5337
5338 case L2CAP_CR_AUTHENTICATION:
5339 case L2CAP_CR_ENCRYPTION:
5340 /* If we already have MITM protection we can't do
5341 * anything.
5342 */
5343 if (hcon->sec_level > BT_SECURITY_MEDIUM) {
5344 l2cap_chan_del(chan, ECONNREFUSED);
5345 break;
5346 }
5347
5348 sec_level = hcon->sec_level + 1;
5349 if (chan->sec_level < sec_level)
5350 chan->sec_level = sec_level;
5351
5352 /* We'll need to send a new Connect Request */
5353 clear_bit(FLAG_LE_CONN_REQ_SENT, &chan->flags);
5354
5355 smp_conn_security(hcon, chan->sec_level);
5356 break;
5357
5358 default:
5359 l2cap_chan_del(chan, ECONNREFUSED);
5360 break;
5361 }
5362
5363 l2cap_chan_unlock(chan);
5364
5365 unlock:
5366 mutex_unlock(&conn->chan_lock);
5367
5368 return err;
5369 }
5370
5371 static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
5372 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5373 u8 *data)
5374 {
5375 int err = 0;
5376
5377 switch (cmd->code) {
5378 case L2CAP_COMMAND_REJ:
5379 l2cap_command_rej(conn, cmd, cmd_len, data);
5380 break;
5381
5382 case L2CAP_CONN_REQ:
5383 err = l2cap_connect_req(conn, cmd, cmd_len, data);
5384 break;
5385
5386 case L2CAP_CONN_RSP:
5387 case L2CAP_CREATE_CHAN_RSP:
5388 l2cap_connect_create_rsp(conn, cmd, cmd_len, data);
5389 break;
5390
5391 case L2CAP_CONF_REQ:
5392 err = l2cap_config_req(conn, cmd, cmd_len, data);
5393 break;
5394
5395 case L2CAP_CONF_RSP:
5396 l2cap_config_rsp(conn, cmd, cmd_len, data);
5397 break;
5398
5399 case L2CAP_DISCONN_REQ:
5400 err = l2cap_disconnect_req(conn, cmd, cmd_len, data);
5401 break;
5402
5403 case L2CAP_DISCONN_RSP:
5404 l2cap_disconnect_rsp(conn, cmd, cmd_len, data);
5405 break;
5406
5407 case L2CAP_ECHO_REQ:
5408 l2cap_send_cmd(conn, cmd->ident, L2CAP_ECHO_RSP, cmd_len, data);
5409 break;
5410
5411 case L2CAP_ECHO_RSP:
5412 break;
5413
5414 case L2CAP_INFO_REQ:
5415 err = l2cap_information_req(conn, cmd, cmd_len, data);
5416 break;
5417
5418 case L2CAP_INFO_RSP:
5419 l2cap_information_rsp(conn, cmd, cmd_len, data);
5420 break;
5421
5422 case L2CAP_CREATE_CHAN_REQ:
5423 err = l2cap_create_channel_req(conn, cmd, cmd_len, data);
5424 break;
5425
5426 case L2CAP_MOVE_CHAN_REQ:
5427 err = l2cap_move_channel_req(conn, cmd, cmd_len, data);
5428 break;
5429
5430 case L2CAP_MOVE_CHAN_RSP:
5431 l2cap_move_channel_rsp(conn, cmd, cmd_len, data);
5432 break;
5433
5434 case L2CAP_MOVE_CHAN_CFM:
5435 err = l2cap_move_channel_confirm(conn, cmd, cmd_len, data);
5436 break;
5437
5438 case L2CAP_MOVE_CHAN_CFM_RSP:
5439 l2cap_move_channel_confirm_rsp(conn, cmd, cmd_len, data);
5440 break;
5441
5442 default:
5443 BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code);
5444 err = -EINVAL;
5445 break;
5446 }
5447
5448 return err;
5449 }
5450
5451 static int l2cap_le_connect_req(struct l2cap_conn *conn,
5452 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5453 u8 *data)
5454 {
5455 struct l2cap_le_conn_req *req = (struct l2cap_le_conn_req *) data;
5456 struct l2cap_le_conn_rsp rsp;
5457 struct l2cap_chan *chan, *pchan;
5458 u16 dcid, scid, credits, mtu, mps;
5459 __le16 psm;
5460 u8 result;
5461
5462 if (cmd_len != sizeof(*req))
5463 return -EPROTO;
5464
5465 scid = __le16_to_cpu(req->scid);
5466 mtu = __le16_to_cpu(req->mtu);
5467 mps = __le16_to_cpu(req->mps);
5468 psm = req->psm;
5469 dcid = 0;
5470 credits = 0;
5471
5472 if (mtu < 23 || mps < 23)
5473 return -EPROTO;
5474
5475 BT_DBG("psm 0x%2.2x scid 0x%4.4x mtu %u mps %u", __le16_to_cpu(psm),
5476 scid, mtu, mps);
5477
5478 /* Check if we have socket listening on psm */
5479 pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, &conn->hcon->src,
5480 &conn->hcon->dst, LE_LINK);
5481 if (!pchan) {
5482 result = L2CAP_CR_BAD_PSM;
5483 chan = NULL;
5484 goto response;
5485 }
5486
5487 mutex_lock(&conn->chan_lock);
5488 l2cap_chan_lock(pchan);
5489
5490 if (!smp_sufficient_security(conn->hcon, pchan->sec_level,
5491 SMP_ALLOW_STK)) {
5492 result = L2CAP_CR_AUTHENTICATION;
5493 chan = NULL;
5494 goto response_unlock;
5495 }
5496
5497 /* Check for valid dynamic CID range */
5498 if (scid < L2CAP_CID_DYN_START || scid > L2CAP_CID_LE_DYN_END) {
5499 result = L2CAP_CR_INVALID_SCID;
5500 chan = NULL;
5501 goto response_unlock;
5502 }
5503
5504 /* Check if we already have channel with that dcid */
5505 if (__l2cap_get_chan_by_dcid(conn, scid)) {
5506 result = L2CAP_CR_SCID_IN_USE;
5507 chan = NULL;
5508 goto response_unlock;
5509 }
5510
5511 chan = pchan->ops->new_connection(pchan);
5512 if (!chan) {
5513 result = L2CAP_CR_NO_MEM;
5514 goto response_unlock;
5515 }
5516
5517 l2cap_le_flowctl_init(chan);
5518
5519 bacpy(&chan->src, &conn->hcon->src);
5520 bacpy(&chan->dst, &conn->hcon->dst);
5521 chan->src_type = bdaddr_src_type(conn->hcon);
5522 chan->dst_type = bdaddr_dst_type(conn->hcon);
5523 chan->psm = psm;
5524 chan->dcid = scid;
5525 chan->omtu = mtu;
5526 chan->remote_mps = mps;
5527 chan->tx_credits = __le16_to_cpu(req->credits);
5528
5529 __l2cap_chan_add(conn, chan);
5530 dcid = chan->scid;
5531 credits = chan->rx_credits;
5532
5533 __set_chan_timer(chan, chan->ops->get_sndtimeo(chan));
5534
5535 chan->ident = cmd->ident;
5536
5537 if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
5538 l2cap_state_change(chan, BT_CONNECT2);
5539 /* The following result value is actually not defined
5540 * for LE CoC but we use it to let the function know
5541 * that it should bail out after doing its cleanup
5542 * instead of sending a response.
5543 */
5544 result = L2CAP_CR_PEND;
5545 chan->ops->defer(chan);
5546 } else {
5547 l2cap_chan_ready(chan);
5548 result = L2CAP_CR_SUCCESS;
5549 }
5550
5551 response_unlock:
5552 l2cap_chan_unlock(pchan);
5553 mutex_unlock(&conn->chan_lock);
5554 l2cap_chan_put(pchan);
5555
5556 if (result == L2CAP_CR_PEND)
5557 return 0;
5558
5559 response:
5560 if (chan) {
5561 rsp.mtu = cpu_to_le16(chan->imtu);
5562 rsp.mps = cpu_to_le16(chan->mps);
5563 } else {
5564 rsp.mtu = 0;
5565 rsp.mps = 0;
5566 }
5567
5568 rsp.dcid = cpu_to_le16(dcid);
5569 rsp.credits = cpu_to_le16(credits);
5570 rsp.result = cpu_to_le16(result);
5571
5572 l2cap_send_cmd(conn, cmd->ident, L2CAP_LE_CONN_RSP, sizeof(rsp), &rsp);
5573
5574 return 0;
5575 }
5576
5577 static inline int l2cap_le_credits(struct l2cap_conn *conn,
5578 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5579 u8 *data)
5580 {
5581 struct l2cap_le_credits *pkt;
5582 struct l2cap_chan *chan;
5583 u16 cid, credits, max_credits;
5584
5585 if (cmd_len != sizeof(*pkt))
5586 return -EPROTO;
5587
5588 pkt = (struct l2cap_le_credits *) data;
5589 cid = __le16_to_cpu(pkt->cid);
5590 credits = __le16_to_cpu(pkt->credits);
5591
5592 BT_DBG("cid 0x%4.4x credits 0x%4.4x", cid, credits);
5593
5594 chan = l2cap_get_chan_by_dcid(conn, cid);
5595 if (!chan)
5596 return -EBADSLT;
5597
5598 max_credits = LE_FLOWCTL_MAX_CREDITS - chan->tx_credits;
5599 if (credits > max_credits) {
5600 BT_ERR("LE credits overflow");
5601 l2cap_send_disconn_req(chan, ECONNRESET);
5602 l2cap_chan_unlock(chan);
5603
5604 /* Return 0 so that we don't trigger an unnecessary
5605 * command reject packet.
5606 */
5607 return 0;
5608 }
5609
5610 chan->tx_credits += credits;
5611
5612 /* Resume sending */
5613 l2cap_le_flowctl_send(chan);
5614
5615 if (chan->tx_credits)
5616 chan->ops->resume(chan);
5617
5618 l2cap_chan_unlock(chan);
5619
5620 return 0;
5621 }
5622
5623 static inline int l2cap_le_command_rej(struct l2cap_conn *conn,
5624 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5625 u8 *data)
5626 {
5627 struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data;
5628 struct l2cap_chan *chan;
5629
5630 if (cmd_len < sizeof(*rej))
5631 return -EPROTO;
5632
5633 mutex_lock(&conn->chan_lock);
5634
5635 chan = __l2cap_get_chan_by_ident(conn, cmd->ident);
5636 if (!chan)
5637 goto done;
5638
5639 l2cap_chan_lock(chan);
5640 l2cap_chan_del(chan, ECONNREFUSED);
5641 l2cap_chan_unlock(chan);
5642
5643 done:
5644 mutex_unlock(&conn->chan_lock);
5645 return 0;
5646 }
5647
5648 static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn,
5649 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5650 u8 *data)
5651 {
5652 int err = 0;
5653
5654 switch (cmd->code) {
5655 case L2CAP_COMMAND_REJ:
5656 l2cap_le_command_rej(conn, cmd, cmd_len, data);
5657 break;
5658
5659 case L2CAP_CONN_PARAM_UPDATE_REQ:
5660 err = l2cap_conn_param_update_req(conn, cmd, cmd_len, data);
5661 break;
5662
5663 case L2CAP_CONN_PARAM_UPDATE_RSP:
5664 break;
5665
5666 case L2CAP_LE_CONN_RSP:
5667 l2cap_le_connect_rsp(conn, cmd, cmd_len, data);
5668 break;
5669
5670 case L2CAP_LE_CONN_REQ:
5671 err = l2cap_le_connect_req(conn, cmd, cmd_len, data);
5672 break;
5673
5674 case L2CAP_LE_CREDITS:
5675 err = l2cap_le_credits(conn, cmd, cmd_len, data);
5676 break;
5677
5678 case L2CAP_DISCONN_REQ:
5679 err = l2cap_disconnect_req(conn, cmd, cmd_len, data);
5680 break;
5681
5682 case L2CAP_DISCONN_RSP:
5683 l2cap_disconnect_rsp(conn, cmd, cmd_len, data);
5684 break;
5685
5686 default:
5687 BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code);
5688 err = -EINVAL;
5689 break;
5690 }
5691
5692 return err;
5693 }
5694
5695 static inline void l2cap_le_sig_channel(struct l2cap_conn *conn,
5696 struct sk_buff *skb)
5697 {
5698 struct hci_conn *hcon = conn->hcon;
5699 struct l2cap_cmd_hdr *cmd;
5700 u16 len;
5701 int err;
5702
5703 if (hcon->type != LE_LINK)
5704 goto drop;
5705
5706 if (skb->len < L2CAP_CMD_HDR_SIZE)
5707 goto drop;
5708
5709 cmd = (void *) skb->data;
5710 skb_pull(skb, L2CAP_CMD_HDR_SIZE);
5711
5712 len = le16_to_cpu(cmd->len);
5713
5714 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd->code, len, cmd->ident);
5715
5716 if (len != skb->len || !cmd->ident) {
5717 BT_DBG("corrupted command");
5718 goto drop;
5719 }
5720
5721 err = l2cap_le_sig_cmd(conn, cmd, len, skb->data);
5722 if (err) {
5723 struct l2cap_cmd_rej_unk rej;
5724
5725 BT_ERR("Wrong link type (%d)", err);
5726
5727 rej.reason = cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
5728 l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ,
5729 sizeof(rej), &rej);
5730 }
5731
5732 drop:
5733 kfree_skb(skb);
5734 }
5735
5736 static inline void l2cap_sig_channel(struct l2cap_conn *conn,
5737 struct sk_buff *skb)
5738 {
5739 struct hci_conn *hcon = conn->hcon;
5740 u8 *data = skb->data;
5741 int len = skb->len;
5742 struct l2cap_cmd_hdr cmd;
5743 int err;
5744
5745 l2cap_raw_recv(conn, skb);
5746
5747 if (hcon->type != ACL_LINK)
5748 goto drop;
5749
5750 while (len >= L2CAP_CMD_HDR_SIZE) {
5751 u16 cmd_len;
5752 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
5753 data += L2CAP_CMD_HDR_SIZE;
5754 len -= L2CAP_CMD_HDR_SIZE;
5755
5756 cmd_len = le16_to_cpu(cmd.len);
5757
5758 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len,
5759 cmd.ident);
5760
5761 if (cmd_len > len || !cmd.ident) {
5762 BT_DBG("corrupted command");
5763 break;
5764 }
5765
5766 err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data);
5767 if (err) {
5768 struct l2cap_cmd_rej_unk rej;
5769
5770 BT_ERR("Wrong link type (%d)", err);
5771
5772 rej.reason = cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
5773 l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ,
5774 sizeof(rej), &rej);
5775 }
5776
5777 data += cmd_len;
5778 len -= cmd_len;
5779 }
5780
5781 drop:
5782 kfree_skb(skb);
5783 }
5784
5785 static int l2cap_check_fcs(struct l2cap_chan *chan, struct sk_buff *skb)
5786 {
5787 u16 our_fcs, rcv_fcs;
5788 int hdr_size;
5789
5790 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
5791 hdr_size = L2CAP_EXT_HDR_SIZE;
5792 else
5793 hdr_size = L2CAP_ENH_HDR_SIZE;
5794
5795 if (chan->fcs == L2CAP_FCS_CRC16) {
5796 skb_trim(skb, skb->len - L2CAP_FCS_SIZE);
5797 rcv_fcs = get_unaligned_le16(skb->data + skb->len);
5798 our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
5799
5800 if (our_fcs != rcv_fcs)
5801 return -EBADMSG;
5802 }
5803 return 0;
5804 }
5805
5806 static void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan)
5807 {
5808 struct l2cap_ctrl control;
5809
5810 BT_DBG("chan %p", chan);
5811
5812 memset(&control, 0, sizeof(control));
5813 control.sframe = 1;
5814 control.final = 1;
5815 control.reqseq = chan->buffer_seq;
5816 set_bit(CONN_SEND_FBIT, &chan->conn_state);
5817
5818 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
5819 control.super = L2CAP_SUPER_RNR;
5820 l2cap_send_sframe(chan, &control);
5821 }
5822
5823 if (test_and_clear_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
5824 chan->unacked_frames > 0)
5825 __set_retrans_timer(chan);
5826
5827 /* Send pending iframes */
5828 l2cap_ertm_send(chan);
5829
5830 if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state) &&
5831 test_bit(CONN_SEND_FBIT, &chan->conn_state)) {
5832 /* F-bit wasn't sent in an s-frame or i-frame yet, so
5833 * send it now.
5834 */
5835 control.super = L2CAP_SUPER_RR;
5836 l2cap_send_sframe(chan, &control);
5837 }
5838 }
5839
5840 static void append_skb_frag(struct sk_buff *skb, struct sk_buff *new_frag,
5841 struct sk_buff **last_frag)
5842 {
5843 /* skb->len reflects data in skb as well as all fragments
5844 * skb->data_len reflects only data in fragments
5845 */
5846 if (!skb_has_frag_list(skb))
5847 skb_shinfo(skb)->frag_list = new_frag;
5848
5849 new_frag->next = NULL;
5850
5851 (*last_frag)->next = new_frag;
5852 *last_frag = new_frag;
5853
5854 skb->len += new_frag->len;
5855 skb->data_len += new_frag->len;
5856 skb->truesize += new_frag->truesize;
5857 }
5858
5859 static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb,
5860 struct l2cap_ctrl *control)
5861 {
5862 int err = -EINVAL;
5863
5864 switch (control->sar) {
5865 case L2CAP_SAR_UNSEGMENTED:
5866 if (chan->sdu)
5867 break;
5868
5869 err = chan->ops->recv(chan, skb);
5870 break;
5871
5872 case L2CAP_SAR_START:
5873 if (chan->sdu)
5874 break;
5875
5876 if (!pskb_may_pull(skb, L2CAP_SDULEN_SIZE))
5877 break;
5878
5879 chan->sdu_len = get_unaligned_le16(skb->data);
5880 skb_pull(skb, L2CAP_SDULEN_SIZE);
5881
5882 if (chan->sdu_len > chan->imtu) {
5883 err = -EMSGSIZE;
5884 break;
5885 }
5886
5887 if (skb->len >= chan->sdu_len)
5888 break;
5889
5890 chan->sdu = skb;
5891 chan->sdu_last_frag = skb;
5892
5893 skb = NULL;
5894 err = 0;
5895 break;
5896
5897 case L2CAP_SAR_CONTINUE:
5898 if (!chan->sdu)
5899 break;
5900
5901 append_skb_frag(chan->sdu, skb,
5902 &chan->sdu_last_frag);
5903 skb = NULL;
5904
5905 if (chan->sdu->len >= chan->sdu_len)
5906 break;
5907
5908 err = 0;
5909 break;
5910
5911 case L2CAP_SAR_END:
5912 if (!chan->sdu)
5913 break;
5914
5915 append_skb_frag(chan->sdu, skb,
5916 &chan->sdu_last_frag);
5917 skb = NULL;
5918
5919 if (chan->sdu->len != chan->sdu_len)
5920 break;
5921
5922 err = chan->ops->recv(chan, chan->sdu);
5923
5924 if (!err) {
5925 /* Reassembly complete */
5926 chan->sdu = NULL;
5927 chan->sdu_last_frag = NULL;
5928 chan->sdu_len = 0;
5929 }
5930 break;
5931 }
5932
5933 if (err) {
5934 kfree_skb(skb);
5935 kfree_skb(chan->sdu);
5936 chan->sdu = NULL;
5937 chan->sdu_last_frag = NULL;
5938 chan->sdu_len = 0;
5939 }
5940
5941 return err;
5942 }
5943
5944 static int l2cap_resegment(struct l2cap_chan *chan)
5945 {
5946 /* Placeholder */
5947 return 0;
5948 }
5949
5950 void l2cap_chan_busy(struct l2cap_chan *chan, int busy)
5951 {
5952 u8 event;
5953
5954 if (chan->mode != L2CAP_MODE_ERTM)
5955 return;
5956
5957 event = busy ? L2CAP_EV_LOCAL_BUSY_DETECTED : L2CAP_EV_LOCAL_BUSY_CLEAR;
5958 l2cap_tx(chan, NULL, NULL, event);
5959 }
5960
5961 static int l2cap_rx_queued_iframes(struct l2cap_chan *chan)
5962 {
5963 int err = 0;
5964 /* Pass sequential frames to l2cap_reassemble_sdu()
5965 * until a gap is encountered.
5966 */
5967
5968 BT_DBG("chan %p", chan);
5969
5970 while (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
5971 struct sk_buff *skb;
5972 BT_DBG("Searching for skb with txseq %d (queue len %d)",
5973 chan->buffer_seq, skb_queue_len(&chan->srej_q));
5974
5975 skb = l2cap_ertm_seq_in_queue(&chan->srej_q, chan->buffer_seq);
5976
5977 if (!skb)
5978 break;
5979
5980 skb_unlink(skb, &chan->srej_q);
5981 chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
5982 err = l2cap_reassemble_sdu(chan, skb, &bt_cb(skb)->l2cap);
5983 if (err)
5984 break;
5985 }
5986
5987 if (skb_queue_empty(&chan->srej_q)) {
5988 chan->rx_state = L2CAP_RX_STATE_RECV;
5989 l2cap_send_ack(chan);
5990 }
5991
5992 return err;
5993 }
5994
5995 static void l2cap_handle_srej(struct l2cap_chan *chan,
5996 struct l2cap_ctrl *control)
5997 {
5998 struct sk_buff *skb;
5999
6000 BT_DBG("chan %p, control %p", chan, control);
6001
6002 if (control->reqseq == chan->next_tx_seq) {
6003 BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq);
6004 l2cap_send_disconn_req(chan, ECONNRESET);
6005 return;
6006 }
6007
6008 skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq);
6009
6010 if (skb == NULL) {
6011 BT_DBG("Seq %d not available for retransmission",
6012 control->reqseq);
6013 return;
6014 }
6015
6016 if (chan->max_tx != 0 && bt_cb(skb)->l2cap.retries >= chan->max_tx) {
6017 BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
6018 l2cap_send_disconn_req(chan, ECONNRESET);
6019 return;
6020 }
6021
6022 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6023
6024 if (control->poll) {
6025 l2cap_pass_to_tx(chan, control);
6026
6027 set_bit(CONN_SEND_FBIT, &chan->conn_state);
6028 l2cap_retransmit(chan, control);
6029 l2cap_ertm_send(chan);
6030
6031 if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) {
6032 set_bit(CONN_SREJ_ACT, &chan->conn_state);
6033 chan->srej_save_reqseq = control->reqseq;
6034 }
6035 } else {
6036 l2cap_pass_to_tx_fbit(chan, control);
6037
6038 if (control->final) {
6039 if (chan->srej_save_reqseq != control->reqseq ||
6040 !test_and_clear_bit(CONN_SREJ_ACT,
6041 &chan->conn_state))
6042 l2cap_retransmit(chan, control);
6043 } else {
6044 l2cap_retransmit(chan, control);
6045 if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) {
6046 set_bit(CONN_SREJ_ACT, &chan->conn_state);
6047 chan->srej_save_reqseq = control->reqseq;
6048 }
6049 }
6050 }
6051 }
6052
6053 static void l2cap_handle_rej(struct l2cap_chan *chan,
6054 struct l2cap_ctrl *control)
6055 {
6056 struct sk_buff *skb;
6057
6058 BT_DBG("chan %p, control %p", chan, control);
6059
6060 if (control->reqseq == chan->next_tx_seq) {
6061 BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq);
6062 l2cap_send_disconn_req(chan, ECONNRESET);
6063 return;
6064 }
6065
6066 skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq);
6067
6068 if (chan->max_tx && skb &&
6069 bt_cb(skb)->l2cap.retries >= chan->max_tx) {
6070 BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
6071 l2cap_send_disconn_req(chan, ECONNRESET);
6072 return;
6073 }
6074
6075 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6076
6077 l2cap_pass_to_tx(chan, control);
6078
6079 if (control->final) {
6080 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
6081 l2cap_retransmit_all(chan, control);
6082 } else {
6083 l2cap_retransmit_all(chan, control);
6084 l2cap_ertm_send(chan);
6085 if (chan->tx_state == L2CAP_TX_STATE_WAIT_F)
6086 set_bit(CONN_REJ_ACT, &chan->conn_state);
6087 }
6088 }
6089
6090 static u8 l2cap_classify_txseq(struct l2cap_chan *chan, u16 txseq)
6091 {
6092 BT_DBG("chan %p, txseq %d", chan, txseq);
6093
6094 BT_DBG("last_acked_seq %d, expected_tx_seq %d", chan->last_acked_seq,
6095 chan->expected_tx_seq);
6096
6097 if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
6098 if (__seq_offset(chan, txseq, chan->last_acked_seq) >=
6099 chan->tx_win) {
6100 /* See notes below regarding "double poll" and
6101 * invalid packets.
6102 */
6103 if (chan->tx_win <= ((chan->tx_win_max + 1) >> 1)) {
6104 BT_DBG("Invalid/Ignore - after SREJ");
6105 return L2CAP_TXSEQ_INVALID_IGNORE;
6106 } else {
6107 BT_DBG("Invalid - in window after SREJ sent");
6108 return L2CAP_TXSEQ_INVALID;
6109 }
6110 }
6111
6112 if (chan->srej_list.head == txseq) {
6113 BT_DBG("Expected SREJ");
6114 return L2CAP_TXSEQ_EXPECTED_SREJ;
6115 }
6116
6117 if (l2cap_ertm_seq_in_queue(&chan->srej_q, txseq)) {
6118 BT_DBG("Duplicate SREJ - txseq already stored");
6119 return L2CAP_TXSEQ_DUPLICATE_SREJ;
6120 }
6121
6122 if (l2cap_seq_list_contains(&chan->srej_list, txseq)) {
6123 BT_DBG("Unexpected SREJ - not requested");
6124 return L2CAP_TXSEQ_UNEXPECTED_SREJ;
6125 }
6126 }
6127
6128 if (chan->expected_tx_seq == txseq) {
6129 if (__seq_offset(chan, txseq, chan->last_acked_seq) >=
6130 chan->tx_win) {
6131 BT_DBG("Invalid - txseq outside tx window");
6132 return L2CAP_TXSEQ_INVALID;
6133 } else {
6134 BT_DBG("Expected");
6135 return L2CAP_TXSEQ_EXPECTED;
6136 }
6137 }
6138
6139 if (__seq_offset(chan, txseq, chan->last_acked_seq) <
6140 __seq_offset(chan, chan->expected_tx_seq, chan->last_acked_seq)) {
6141 BT_DBG("Duplicate - expected_tx_seq later than txseq");
6142 return L2CAP_TXSEQ_DUPLICATE;
6143 }
6144
6145 if (__seq_offset(chan, txseq, chan->last_acked_seq) >= chan->tx_win) {
6146 /* A source of invalid packets is a "double poll" condition,
6147 * where delays cause us to send multiple poll packets. If
6148 * the remote stack receives and processes both polls,
6149 * sequence numbers can wrap around in such a way that a
6150 * resent frame has a sequence number that looks like new data
6151 * with a sequence gap. This would trigger an erroneous SREJ
6152 * request.
6153 *
6154 * Fortunately, this is impossible with a tx window that's
6155 * less than half of the maximum sequence number, which allows
6156 * invalid frames to be safely ignored.
6157 *
6158 * With tx window sizes greater than half of the tx window
6159 * maximum, the frame is invalid and cannot be ignored. This
6160 * causes a disconnect.
6161 */
6162
6163 if (chan->tx_win <= ((chan->tx_win_max + 1) >> 1)) {
6164 BT_DBG("Invalid/Ignore - txseq outside tx window");
6165 return L2CAP_TXSEQ_INVALID_IGNORE;
6166 } else {
6167 BT_DBG("Invalid - txseq outside tx window");
6168 return L2CAP_TXSEQ_INVALID;
6169 }
6170 } else {
6171 BT_DBG("Unexpected - txseq indicates missing frames");
6172 return L2CAP_TXSEQ_UNEXPECTED;
6173 }
6174 }
6175
6176 static int l2cap_rx_state_recv(struct l2cap_chan *chan,
6177 struct l2cap_ctrl *control,
6178 struct sk_buff *skb, u8 event)
6179 {
6180 int err = 0;
6181 bool skb_in_use = false;
6182
6183 BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
6184 event);
6185
6186 switch (event) {
6187 case L2CAP_EV_RECV_IFRAME:
6188 switch (l2cap_classify_txseq(chan, control->txseq)) {
6189 case L2CAP_TXSEQ_EXPECTED:
6190 l2cap_pass_to_tx(chan, control);
6191
6192 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
6193 BT_DBG("Busy, discarding expected seq %d",
6194 control->txseq);
6195 break;
6196 }
6197
6198 chan->expected_tx_seq = __next_seq(chan,
6199 control->txseq);
6200
6201 chan->buffer_seq = chan->expected_tx_seq;
6202 skb_in_use = true;
6203
6204 err = l2cap_reassemble_sdu(chan, skb, control);
6205 if (err)
6206 break;
6207
6208 if (control->final) {
6209 if (!test_and_clear_bit(CONN_REJ_ACT,
6210 &chan->conn_state)) {
6211 control->final = 0;
6212 l2cap_retransmit_all(chan, control);
6213 l2cap_ertm_send(chan);
6214 }
6215 }
6216
6217 if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state))
6218 l2cap_send_ack(chan);
6219 break;
6220 case L2CAP_TXSEQ_UNEXPECTED:
6221 l2cap_pass_to_tx(chan, control);
6222
6223 /* Can't issue SREJ frames in the local busy state.
6224 * Drop this frame, it will be seen as missing
6225 * when local busy is exited.
6226 */
6227 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
6228 BT_DBG("Busy, discarding unexpected seq %d",
6229 control->txseq);
6230 break;
6231 }
6232
6233 /* There was a gap in the sequence, so an SREJ
6234 * must be sent for each missing frame. The
6235 * current frame is stored for later use.
6236 */
6237 skb_queue_tail(&chan->srej_q, skb);
6238 skb_in_use = true;
6239 BT_DBG("Queued %p (queue len %d)", skb,
6240 skb_queue_len(&chan->srej_q));
6241
6242 clear_bit(CONN_SREJ_ACT, &chan->conn_state);
6243 l2cap_seq_list_clear(&chan->srej_list);
6244 l2cap_send_srej(chan, control->txseq);
6245
6246 chan->rx_state = L2CAP_RX_STATE_SREJ_SENT;
6247 break;
6248 case L2CAP_TXSEQ_DUPLICATE:
6249 l2cap_pass_to_tx(chan, control);
6250 break;
6251 case L2CAP_TXSEQ_INVALID_IGNORE:
6252 break;
6253 case L2CAP_TXSEQ_INVALID:
6254 default:
6255 l2cap_send_disconn_req(chan, ECONNRESET);
6256 break;
6257 }
6258 break;
6259 case L2CAP_EV_RECV_RR:
6260 l2cap_pass_to_tx(chan, control);
6261 if (control->final) {
6262 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6263
6264 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state) &&
6265 !__chan_is_moving(chan)) {
6266 control->final = 0;
6267 l2cap_retransmit_all(chan, control);
6268 }
6269
6270 l2cap_ertm_send(chan);
6271 } else if (control->poll) {
6272 l2cap_send_i_or_rr_or_rnr(chan);
6273 } else {
6274 if (test_and_clear_bit(CONN_REMOTE_BUSY,
6275 &chan->conn_state) &&
6276 chan->unacked_frames)
6277 __set_retrans_timer(chan);
6278
6279 l2cap_ertm_send(chan);
6280 }
6281 break;
6282 case L2CAP_EV_RECV_RNR:
6283 set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6284 l2cap_pass_to_tx(chan, control);
6285 if (control && control->poll) {
6286 set_bit(CONN_SEND_FBIT, &chan->conn_state);
6287 l2cap_send_rr_or_rnr(chan, 0);
6288 }
6289 __clear_retrans_timer(chan);
6290 l2cap_seq_list_clear(&chan->retrans_list);
6291 break;
6292 case L2CAP_EV_RECV_REJ:
6293 l2cap_handle_rej(chan, control);
6294 break;
6295 case L2CAP_EV_RECV_SREJ:
6296 l2cap_handle_srej(chan, control);
6297 break;
6298 default:
6299 break;
6300 }
6301
6302 if (skb && !skb_in_use) {
6303 BT_DBG("Freeing %p", skb);
6304 kfree_skb(skb);
6305 }
6306
6307 return err;
6308 }
6309
6310 static int l2cap_rx_state_srej_sent(struct l2cap_chan *chan,
6311 struct l2cap_ctrl *control,
6312 struct sk_buff *skb, u8 event)
6313 {
6314 int err = 0;
6315 u16 txseq = control->txseq;
6316 bool skb_in_use = false;
6317
6318 BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
6319 event);
6320
6321 switch (event) {
6322 case L2CAP_EV_RECV_IFRAME:
6323 switch (l2cap_classify_txseq(chan, txseq)) {
6324 case L2CAP_TXSEQ_EXPECTED:
6325 /* Keep frame for reassembly later */
6326 l2cap_pass_to_tx(chan, control);
6327 skb_queue_tail(&chan->srej_q, skb);
6328 skb_in_use = true;
6329 BT_DBG("Queued %p (queue len %d)", skb,
6330 skb_queue_len(&chan->srej_q));
6331
6332 chan->expected_tx_seq = __next_seq(chan, txseq);
6333 break;
6334 case L2CAP_TXSEQ_EXPECTED_SREJ:
6335 l2cap_seq_list_pop(&chan->srej_list);
6336
6337 l2cap_pass_to_tx(chan, control);
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 err = l2cap_rx_queued_iframes(chan);
6344 if (err)
6345 break;
6346
6347 break;
6348 case L2CAP_TXSEQ_UNEXPECTED:
6349 /* Got a frame that can't be reassembled yet.
6350 * Save it for later, and send SREJs to cover
6351 * the missing frames.
6352 */
6353 skb_queue_tail(&chan->srej_q, skb);
6354 skb_in_use = true;
6355 BT_DBG("Queued %p (queue len %d)", skb,
6356 skb_queue_len(&chan->srej_q));
6357
6358 l2cap_pass_to_tx(chan, control);
6359 l2cap_send_srej(chan, control->txseq);
6360 break;
6361 case L2CAP_TXSEQ_UNEXPECTED_SREJ:
6362 /* This frame was requested with an SREJ, but
6363 * some expected retransmitted frames are
6364 * missing. Request retransmission of missing
6365 * SREJ'd frames.
6366 */
6367 skb_queue_tail(&chan->srej_q, skb);
6368 skb_in_use = true;
6369 BT_DBG("Queued %p (queue len %d)", skb,
6370 skb_queue_len(&chan->srej_q));
6371
6372 l2cap_pass_to_tx(chan, control);
6373 l2cap_send_srej_list(chan, control->txseq);
6374 break;
6375 case L2CAP_TXSEQ_DUPLICATE_SREJ:
6376 /* We've already queued this frame. Drop this copy. */
6377 l2cap_pass_to_tx(chan, control);
6378 break;
6379 case L2CAP_TXSEQ_DUPLICATE:
6380 /* Expecting a later sequence number, so this frame
6381 * was already received. Ignore it completely.
6382 */
6383 break;
6384 case L2CAP_TXSEQ_INVALID_IGNORE:
6385 break;
6386 case L2CAP_TXSEQ_INVALID:
6387 default:
6388 l2cap_send_disconn_req(chan, ECONNRESET);
6389 break;
6390 }
6391 break;
6392 case L2CAP_EV_RECV_RR:
6393 l2cap_pass_to_tx(chan, control);
6394 if (control->final) {
6395 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6396
6397 if (!test_and_clear_bit(CONN_REJ_ACT,
6398 &chan->conn_state)) {
6399 control->final = 0;
6400 l2cap_retransmit_all(chan, control);
6401 }
6402
6403 l2cap_ertm_send(chan);
6404 } else if (control->poll) {
6405 if (test_and_clear_bit(CONN_REMOTE_BUSY,
6406 &chan->conn_state) &&
6407 chan->unacked_frames) {
6408 __set_retrans_timer(chan);
6409 }
6410
6411 set_bit(CONN_SEND_FBIT, &chan->conn_state);
6412 l2cap_send_srej_tail(chan);
6413 } else {
6414 if (test_and_clear_bit(CONN_REMOTE_BUSY,
6415 &chan->conn_state) &&
6416 chan->unacked_frames)
6417 __set_retrans_timer(chan);
6418
6419 l2cap_send_ack(chan);
6420 }
6421 break;
6422 case L2CAP_EV_RECV_RNR:
6423 set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6424 l2cap_pass_to_tx(chan, control);
6425 if (control->poll) {
6426 l2cap_send_srej_tail(chan);
6427 } else {
6428 struct l2cap_ctrl rr_control;
6429 memset(&rr_control, 0, sizeof(rr_control));
6430 rr_control.sframe = 1;
6431 rr_control.super = L2CAP_SUPER_RR;
6432 rr_control.reqseq = chan->buffer_seq;
6433 l2cap_send_sframe(chan, &rr_control);
6434 }
6435
6436 break;
6437 case L2CAP_EV_RECV_REJ:
6438 l2cap_handle_rej(chan, control);
6439 break;
6440 case L2CAP_EV_RECV_SREJ:
6441 l2cap_handle_srej(chan, control);
6442 break;
6443 }
6444
6445 if (skb && !skb_in_use) {
6446 BT_DBG("Freeing %p", skb);
6447 kfree_skb(skb);
6448 }
6449
6450 return err;
6451 }
6452
6453 static int l2cap_finish_move(struct l2cap_chan *chan)
6454 {
6455 BT_DBG("chan %p", chan);
6456
6457 chan->rx_state = L2CAP_RX_STATE_RECV;
6458
6459 if (chan->hs_hcon)
6460 chan->conn->mtu = chan->hs_hcon->hdev->block_mtu;
6461 else
6462 chan->conn->mtu = chan->conn->hcon->hdev->acl_mtu;
6463
6464 return l2cap_resegment(chan);
6465 }
6466
6467 static int l2cap_rx_state_wait_p(struct l2cap_chan *chan,
6468 struct l2cap_ctrl *control,
6469 struct sk_buff *skb, u8 event)
6470 {
6471 int err;
6472
6473 BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
6474 event);
6475
6476 if (!control->poll)
6477 return -EPROTO;
6478
6479 l2cap_process_reqseq(chan, control->reqseq);
6480
6481 if (!skb_queue_empty(&chan->tx_q))
6482 chan->tx_send_head = skb_peek(&chan->tx_q);
6483 else
6484 chan->tx_send_head = NULL;
6485
6486 /* Rewind next_tx_seq to the point expected
6487 * by the receiver.
6488 */
6489 chan->next_tx_seq = control->reqseq;
6490 chan->unacked_frames = 0;
6491
6492 err = l2cap_finish_move(chan);
6493 if (err)
6494 return err;
6495
6496 set_bit(CONN_SEND_FBIT, &chan->conn_state);
6497 l2cap_send_i_or_rr_or_rnr(chan);
6498
6499 if (event == L2CAP_EV_RECV_IFRAME)
6500 return -EPROTO;
6501
6502 return l2cap_rx_state_recv(chan, control, NULL, event);
6503 }
6504
6505 static int l2cap_rx_state_wait_f(struct l2cap_chan *chan,
6506 struct l2cap_ctrl *control,
6507 struct sk_buff *skb, u8 event)
6508 {
6509 int err;
6510
6511 if (!control->final)
6512 return -EPROTO;
6513
6514 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6515
6516 chan->rx_state = L2CAP_RX_STATE_RECV;
6517 l2cap_process_reqseq(chan, control->reqseq);
6518
6519 if (!skb_queue_empty(&chan->tx_q))
6520 chan->tx_send_head = skb_peek(&chan->tx_q);
6521 else
6522 chan->tx_send_head = NULL;
6523
6524 /* Rewind next_tx_seq to the point expected
6525 * by the receiver.
6526 */
6527 chan->next_tx_seq = control->reqseq;
6528 chan->unacked_frames = 0;
6529
6530 if (chan->hs_hcon)
6531 chan->conn->mtu = chan->hs_hcon->hdev->block_mtu;
6532 else
6533 chan->conn->mtu = chan->conn->hcon->hdev->acl_mtu;
6534
6535 err = l2cap_resegment(chan);
6536
6537 if (!err)
6538 err = l2cap_rx_state_recv(chan, control, skb, event);
6539
6540 return err;
6541 }
6542
6543 static bool __valid_reqseq(struct l2cap_chan *chan, u16 reqseq)
6544 {
6545 /* Make sure reqseq is for a packet that has been sent but not acked */
6546 u16 unacked;
6547
6548 unacked = __seq_offset(chan, chan->next_tx_seq, chan->expected_ack_seq);
6549 return __seq_offset(chan, chan->next_tx_seq, reqseq) <= unacked;
6550 }
6551
6552 static int l2cap_rx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
6553 struct sk_buff *skb, u8 event)
6554 {
6555 int err = 0;
6556
6557 BT_DBG("chan %p, control %p, skb %p, event %d, state %d", chan,
6558 control, skb, event, chan->rx_state);
6559
6560 if (__valid_reqseq(chan, control->reqseq)) {
6561 switch (chan->rx_state) {
6562 case L2CAP_RX_STATE_RECV:
6563 err = l2cap_rx_state_recv(chan, control, skb, event);
6564 break;
6565 case L2CAP_RX_STATE_SREJ_SENT:
6566 err = l2cap_rx_state_srej_sent(chan, control, skb,
6567 event);
6568 break;
6569 case L2CAP_RX_STATE_WAIT_P:
6570 err = l2cap_rx_state_wait_p(chan, control, skb, event);
6571 break;
6572 case L2CAP_RX_STATE_WAIT_F:
6573 err = l2cap_rx_state_wait_f(chan, control, skb, event);
6574 break;
6575 default:
6576 /* shut it down */
6577 break;
6578 }
6579 } else {
6580 BT_DBG("Invalid reqseq %d (next_tx_seq %d, expected_ack_seq %d",
6581 control->reqseq, chan->next_tx_seq,
6582 chan->expected_ack_seq);
6583 l2cap_send_disconn_req(chan, ECONNRESET);
6584 }
6585
6586 return err;
6587 }
6588
6589 static int l2cap_stream_rx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
6590 struct sk_buff *skb)
6591 {
6592 BT_DBG("chan %p, control %p, skb %p, state %d", chan, control, skb,
6593 chan->rx_state);
6594
6595 if (l2cap_classify_txseq(chan, control->txseq) ==
6596 L2CAP_TXSEQ_EXPECTED) {
6597 l2cap_pass_to_tx(chan, control);
6598
6599 BT_DBG("buffer_seq %d->%d", chan->buffer_seq,
6600 __next_seq(chan, chan->buffer_seq));
6601
6602 chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
6603
6604 l2cap_reassemble_sdu(chan, skb, control);
6605 } else {
6606 if (chan->sdu) {
6607 kfree_skb(chan->sdu);
6608 chan->sdu = NULL;
6609 }
6610 chan->sdu_last_frag = NULL;
6611 chan->sdu_len = 0;
6612
6613 if (skb) {
6614 BT_DBG("Freeing %p", skb);
6615 kfree_skb(skb);
6616 }
6617 }
6618
6619 chan->last_acked_seq = control->txseq;
6620 chan->expected_tx_seq = __next_seq(chan, control->txseq);
6621
6622 return 0;
6623 }
6624
6625 static int l2cap_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb)
6626 {
6627 struct l2cap_ctrl *control = &bt_cb(skb)->l2cap;
6628 u16 len;
6629 u8 event;
6630
6631 __unpack_control(chan, skb);
6632
6633 len = skb->len;
6634
6635 /*
6636 * We can just drop the corrupted I-frame here.
6637 * Receiver will miss it and start proper recovery
6638 * procedures and ask for retransmission.
6639 */
6640 if (l2cap_check_fcs(chan, skb))
6641 goto drop;
6642
6643 if (!control->sframe && control->sar == L2CAP_SAR_START)
6644 len -= L2CAP_SDULEN_SIZE;
6645
6646 if (chan->fcs == L2CAP_FCS_CRC16)
6647 len -= L2CAP_FCS_SIZE;
6648
6649 if (len > chan->mps) {
6650 l2cap_send_disconn_req(chan, ECONNRESET);
6651 goto drop;
6652 }
6653
6654 if ((chan->mode == L2CAP_MODE_ERTM ||
6655 chan->mode == L2CAP_MODE_STREAMING) && sk_filter(chan->data, skb))
6656 goto drop;
6657
6658 if (!control->sframe) {
6659 int err;
6660
6661 BT_DBG("iframe sar %d, reqseq %d, final %d, txseq %d",
6662 control->sar, control->reqseq, control->final,
6663 control->txseq);
6664
6665 /* Validate F-bit - F=0 always valid, F=1 only
6666 * valid in TX WAIT_F
6667 */
6668 if (control->final && chan->tx_state != L2CAP_TX_STATE_WAIT_F)
6669 goto drop;
6670
6671 if (chan->mode != L2CAP_MODE_STREAMING) {
6672 event = L2CAP_EV_RECV_IFRAME;
6673 err = l2cap_rx(chan, control, skb, event);
6674 } else {
6675 err = l2cap_stream_rx(chan, control, skb);
6676 }
6677
6678 if (err)
6679 l2cap_send_disconn_req(chan, ECONNRESET);
6680 } else {
6681 const u8 rx_func_to_event[4] = {
6682 L2CAP_EV_RECV_RR, L2CAP_EV_RECV_REJ,
6683 L2CAP_EV_RECV_RNR, L2CAP_EV_RECV_SREJ
6684 };
6685
6686 /* Only I-frames are expected in streaming mode */
6687 if (chan->mode == L2CAP_MODE_STREAMING)
6688 goto drop;
6689
6690 BT_DBG("sframe reqseq %d, final %d, poll %d, super %d",
6691 control->reqseq, control->final, control->poll,
6692 control->super);
6693
6694 if (len != 0) {
6695 BT_ERR("Trailing bytes: %d in sframe", len);
6696 l2cap_send_disconn_req(chan, ECONNRESET);
6697 goto drop;
6698 }
6699
6700 /* Validate F and P bits */
6701 if (control->final && (control->poll ||
6702 chan->tx_state != L2CAP_TX_STATE_WAIT_F))
6703 goto drop;
6704
6705 event = rx_func_to_event[control->super];
6706 if (l2cap_rx(chan, control, skb, event))
6707 l2cap_send_disconn_req(chan, ECONNRESET);
6708 }
6709
6710 return 0;
6711
6712 drop:
6713 kfree_skb(skb);
6714 return 0;
6715 }
6716
6717 static void l2cap_chan_le_send_credits(struct l2cap_chan *chan)
6718 {
6719 struct l2cap_conn *conn = chan->conn;
6720 struct l2cap_le_credits pkt;
6721 u16 return_credits;
6722
6723 /* We return more credits to the sender only after the amount of
6724 * credits falls below half of the initial amount.
6725 */
6726 if (chan->rx_credits >= (le_max_credits + 1) / 2)
6727 return;
6728
6729 return_credits = le_max_credits - chan->rx_credits;
6730
6731 BT_DBG("chan %p returning %u credits to sender", chan, return_credits);
6732
6733 chan->rx_credits += return_credits;
6734
6735 pkt.cid = cpu_to_le16(chan->scid);
6736 pkt.credits = cpu_to_le16(return_credits);
6737
6738 chan->ident = l2cap_get_ident(conn);
6739
6740 l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CREDITS, sizeof(pkt), &pkt);
6741 }
6742
6743 static int l2cap_le_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb)
6744 {
6745 int err;
6746
6747 if (!chan->rx_credits) {
6748 BT_ERR("No credits to receive LE L2CAP data");
6749 l2cap_send_disconn_req(chan, ECONNRESET);
6750 return -ENOBUFS;
6751 }
6752
6753 if (chan->imtu < skb->len) {
6754 BT_ERR("Too big LE L2CAP PDU");
6755 return -ENOBUFS;
6756 }
6757
6758 chan->rx_credits--;
6759 BT_DBG("rx_credits %u -> %u", chan->rx_credits + 1, chan->rx_credits);
6760
6761 l2cap_chan_le_send_credits(chan);
6762
6763 err = 0;
6764
6765 if (!chan->sdu) {
6766 u16 sdu_len;
6767
6768 sdu_len = get_unaligned_le16(skb->data);
6769 skb_pull(skb, L2CAP_SDULEN_SIZE);
6770
6771 BT_DBG("Start of new SDU. sdu_len %u skb->len %u imtu %u",
6772 sdu_len, skb->len, chan->imtu);
6773
6774 if (sdu_len > chan->imtu) {
6775 BT_ERR("Too big LE L2CAP SDU length received");
6776 err = -EMSGSIZE;
6777 goto failed;
6778 }
6779
6780 if (skb->len > sdu_len) {
6781 BT_ERR("Too much LE L2CAP data received");
6782 err = -EINVAL;
6783 goto failed;
6784 }
6785
6786 if (skb->len == sdu_len)
6787 return chan->ops->recv(chan, skb);
6788
6789 chan->sdu = skb;
6790 chan->sdu_len = sdu_len;
6791 chan->sdu_last_frag = skb;
6792
6793 return 0;
6794 }
6795
6796 BT_DBG("SDU fragment. chan->sdu->len %u skb->len %u chan->sdu_len %u",
6797 chan->sdu->len, skb->len, chan->sdu_len);
6798
6799 if (chan->sdu->len + skb->len > chan->sdu_len) {
6800 BT_ERR("Too much LE L2CAP data received");
6801 err = -EINVAL;
6802 goto failed;
6803 }
6804
6805 append_skb_frag(chan->sdu, skb, &chan->sdu_last_frag);
6806 skb = NULL;
6807
6808 if (chan->sdu->len == chan->sdu_len) {
6809 err = chan->ops->recv(chan, chan->sdu);
6810 if (!err) {
6811 chan->sdu = NULL;
6812 chan->sdu_last_frag = NULL;
6813 chan->sdu_len = 0;
6814 }
6815 }
6816
6817 failed:
6818 if (err) {
6819 kfree_skb(skb);
6820 kfree_skb(chan->sdu);
6821 chan->sdu = NULL;
6822 chan->sdu_last_frag = NULL;
6823 chan->sdu_len = 0;
6824 }
6825
6826 /* We can't return an error here since we took care of the skb
6827 * freeing internally. An error return would cause the caller to
6828 * do a double-free of the skb.
6829 */
6830 return 0;
6831 }
6832
6833 static void l2cap_data_channel(struct l2cap_conn *conn, u16 cid,
6834 struct sk_buff *skb)
6835 {
6836 struct l2cap_chan *chan;
6837
6838 chan = l2cap_get_chan_by_scid(conn, cid);
6839 if (!chan) {
6840 if (cid == L2CAP_CID_A2MP) {
6841 chan = a2mp_channel_create(conn, skb);
6842 if (!chan) {
6843 kfree_skb(skb);
6844 return;
6845 }
6846
6847 l2cap_chan_lock(chan);
6848 } else {
6849 BT_DBG("unknown cid 0x%4.4x", cid);
6850 /* Drop packet and return */
6851 kfree_skb(skb);
6852 return;
6853 }
6854 }
6855
6856 BT_DBG("chan %p, len %d", chan, skb->len);
6857
6858 /* If we receive data on a fixed channel before the info req/rsp
6859 * procdure is done simply assume that the channel is supported
6860 * and mark it as ready.
6861 */
6862 if (chan->chan_type == L2CAP_CHAN_FIXED)
6863 l2cap_chan_ready(chan);
6864
6865 if (chan->state != BT_CONNECTED)
6866 goto drop;
6867
6868 switch (chan->mode) {
6869 case L2CAP_MODE_LE_FLOWCTL:
6870 if (l2cap_le_data_rcv(chan, skb) < 0)
6871 goto drop;
6872
6873 goto done;
6874
6875 case L2CAP_MODE_BASIC:
6876 /* If socket recv buffers overflows we drop data here
6877 * which is *bad* because L2CAP has to be reliable.
6878 * But we don't have any other choice. L2CAP doesn't
6879 * provide flow control mechanism. */
6880
6881 if (chan->imtu < skb->len) {
6882 BT_ERR("Dropping L2CAP data: receive buffer overflow");
6883 goto drop;
6884 }
6885
6886 if (!chan->ops->recv(chan, skb))
6887 goto done;
6888 break;
6889
6890 case L2CAP_MODE_ERTM:
6891 case L2CAP_MODE_STREAMING:
6892 l2cap_data_rcv(chan, skb);
6893 goto done;
6894
6895 default:
6896 BT_DBG("chan %p: bad mode 0x%2.2x", chan, chan->mode);
6897 break;
6898 }
6899
6900 drop:
6901 kfree_skb(skb);
6902
6903 done:
6904 l2cap_chan_unlock(chan);
6905 }
6906
6907 static void l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm,
6908 struct sk_buff *skb)
6909 {
6910 struct hci_conn *hcon = conn->hcon;
6911 struct l2cap_chan *chan;
6912
6913 if (hcon->type != ACL_LINK)
6914 goto free_skb;
6915
6916 chan = l2cap_global_chan_by_psm(0, psm, &hcon->src, &hcon->dst,
6917 ACL_LINK);
6918 if (!chan)
6919 goto free_skb;
6920
6921 BT_DBG("chan %p, len %d", chan, skb->len);
6922
6923 if (chan->state != BT_BOUND && chan->state != BT_CONNECTED)
6924 goto drop;
6925
6926 if (chan->imtu < skb->len)
6927 goto drop;
6928
6929 /* Store remote BD_ADDR and PSM for msg_name */
6930 bacpy(&bt_cb(skb)->l2cap.bdaddr, &hcon->dst);
6931 bt_cb(skb)->l2cap.psm = psm;
6932
6933 if (!chan->ops->recv(chan, skb)) {
6934 l2cap_chan_put(chan);
6935 return;
6936 }
6937
6938 drop:
6939 l2cap_chan_put(chan);
6940 free_skb:
6941 kfree_skb(skb);
6942 }
6943
6944 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
6945 {
6946 struct l2cap_hdr *lh = (void *) skb->data;
6947 struct hci_conn *hcon = conn->hcon;
6948 u16 cid, len;
6949 __le16 psm;
6950
6951 if (hcon->state != BT_CONNECTED) {
6952 BT_DBG("queueing pending rx skb");
6953 skb_queue_tail(&conn->pending_rx, skb);
6954 return;
6955 }
6956
6957 skb_pull(skb, L2CAP_HDR_SIZE);
6958 cid = __le16_to_cpu(lh->cid);
6959 len = __le16_to_cpu(lh->len);
6960
6961 if (len != skb->len) {
6962 kfree_skb(skb);
6963 return;
6964 }
6965
6966 /* Since we can't actively block incoming LE connections we must
6967 * at least ensure that we ignore incoming data from them.
6968 */
6969 if (hcon->type == LE_LINK &&
6970 hci_bdaddr_list_lookup(&hcon->hdev->blacklist, &hcon->dst,
6971 bdaddr_dst_type(hcon))) {
6972 kfree_skb(skb);
6973 return;
6974 }
6975
6976 BT_DBG("len %d, cid 0x%4.4x", len, cid);
6977
6978 switch (cid) {
6979 case L2CAP_CID_SIGNALING:
6980 l2cap_sig_channel(conn, skb);
6981 break;
6982
6983 case L2CAP_CID_CONN_LESS:
6984 psm = get_unaligned((__le16 *) skb->data);
6985 skb_pull(skb, L2CAP_PSMLEN_SIZE);
6986 l2cap_conless_channel(conn, psm, skb);
6987 break;
6988
6989 case L2CAP_CID_LE_SIGNALING:
6990 l2cap_le_sig_channel(conn, skb);
6991 break;
6992
6993 default:
6994 l2cap_data_channel(conn, cid, skb);
6995 break;
6996 }
6997 }
6998
6999 static void process_pending_rx(struct work_struct *work)
7000 {
7001 struct l2cap_conn *conn = container_of(work, struct l2cap_conn,
7002 pending_rx_work);
7003 struct sk_buff *skb;
7004
7005 BT_DBG("");
7006
7007 while ((skb = skb_dequeue(&conn->pending_rx)))
7008 l2cap_recv_frame(conn, skb);
7009 }
7010
7011 static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon)
7012 {
7013 struct l2cap_conn *conn = hcon->l2cap_data;
7014 struct hci_chan *hchan;
7015
7016 if (conn)
7017 return conn;
7018
7019 hchan = hci_chan_create(hcon);
7020 if (!hchan)
7021 return NULL;
7022
7023 conn = kzalloc(sizeof(*conn), GFP_KERNEL);
7024 if (!conn) {
7025 hci_chan_del(hchan);
7026 return NULL;
7027 }
7028
7029 kref_init(&conn->ref);
7030 hcon->l2cap_data = conn;
7031 conn->hcon = hci_conn_get(hcon);
7032 conn->hchan = hchan;
7033
7034 BT_DBG("hcon %p conn %p hchan %p", hcon, conn, hchan);
7035
7036 switch (hcon->type) {
7037 case LE_LINK:
7038 if (hcon->hdev->le_mtu) {
7039 conn->mtu = hcon->hdev->le_mtu;
7040 break;
7041 }
7042 /* fall through */
7043 default:
7044 conn->mtu = hcon->hdev->acl_mtu;
7045 break;
7046 }
7047
7048 conn->feat_mask = 0;
7049
7050 conn->local_fixed_chan = L2CAP_FC_SIG_BREDR | L2CAP_FC_CONNLESS;
7051
7052 if (hcon->type == ACL_LINK &&
7053 hci_dev_test_flag(hcon->hdev, HCI_HS_ENABLED))
7054 conn->local_fixed_chan |= L2CAP_FC_A2MP;
7055
7056 if (hci_dev_test_flag(hcon->hdev, HCI_LE_ENABLED) &&
7057 (bredr_sc_enabled(hcon->hdev) ||
7058 hci_dev_test_flag(hcon->hdev, HCI_FORCE_BREDR_SMP)))
7059 conn->local_fixed_chan |= L2CAP_FC_SMP_BREDR;
7060
7061 mutex_init(&conn->ident_lock);
7062 mutex_init(&conn->chan_lock);
7063
7064 INIT_LIST_HEAD(&conn->chan_l);
7065 INIT_LIST_HEAD(&conn->users);
7066
7067 INIT_DELAYED_WORK(&conn->info_timer, l2cap_info_timeout);
7068
7069 skb_queue_head_init(&conn->pending_rx);
7070 INIT_WORK(&conn->pending_rx_work, process_pending_rx);
7071 INIT_WORK(&conn->id_addr_update_work, l2cap_conn_update_id_addr);
7072
7073 conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM;
7074
7075 return conn;
7076 }
7077
7078 static bool is_valid_psm(u16 psm, u8 dst_type) {
7079 if (!psm)
7080 return false;
7081
7082 if (bdaddr_type_is_le(dst_type))
7083 return (psm <= 0x00ff);
7084
7085 /* PSM must be odd and lsb of upper byte must be 0 */
7086 return ((psm & 0x0101) == 0x0001);
7087 }
7088
7089 int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid,
7090 bdaddr_t *dst, u8 dst_type)
7091 {
7092 struct l2cap_conn *conn;
7093 struct hci_conn *hcon;
7094 struct hci_dev *hdev;
7095 int err;
7096
7097 BT_DBG("%pMR -> %pMR (type %u) psm 0x%2.2x", &chan->src, dst,
7098 dst_type, __le16_to_cpu(psm));
7099
7100 hdev = hci_get_route(dst, &chan->src, chan->src_type);
7101 if (!hdev)
7102 return -EHOSTUNREACH;
7103
7104 hci_dev_lock(hdev);
7105
7106 if (!is_valid_psm(__le16_to_cpu(psm), dst_type) && !cid &&
7107 chan->chan_type != L2CAP_CHAN_RAW) {
7108 err = -EINVAL;
7109 goto done;
7110 }
7111
7112 if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED && !psm) {
7113 err = -EINVAL;
7114 goto done;
7115 }
7116
7117 if (chan->chan_type == L2CAP_CHAN_FIXED && !cid) {
7118 err = -EINVAL;
7119 goto done;
7120 }
7121
7122 switch (chan->mode) {
7123 case L2CAP_MODE_BASIC:
7124 break;
7125 case L2CAP_MODE_LE_FLOWCTL:
7126 l2cap_le_flowctl_init(chan);
7127 break;
7128 case L2CAP_MODE_ERTM:
7129 case L2CAP_MODE_STREAMING:
7130 if (!disable_ertm)
7131 break;
7132 /* fall through */
7133 default:
7134 err = -EOPNOTSUPP;
7135 goto done;
7136 }
7137
7138 switch (chan->state) {
7139 case BT_CONNECT:
7140 case BT_CONNECT2:
7141 case BT_CONFIG:
7142 /* Already connecting */
7143 err = 0;
7144 goto done;
7145
7146 case BT_CONNECTED:
7147 /* Already connected */
7148 err = -EISCONN;
7149 goto done;
7150
7151 case BT_OPEN:
7152 case BT_BOUND:
7153 /* Can connect */
7154 break;
7155
7156 default:
7157 err = -EBADFD;
7158 goto done;
7159 }
7160
7161 /* Set destination address and psm */
7162 bacpy(&chan->dst, dst);
7163 chan->dst_type = dst_type;
7164
7165 chan->psm = psm;
7166 chan->dcid = cid;
7167
7168 if (bdaddr_type_is_le(dst_type)) {
7169 /* Convert from L2CAP channel address type to HCI address type
7170 */
7171 if (dst_type == BDADDR_LE_PUBLIC)
7172 dst_type = ADDR_LE_DEV_PUBLIC;
7173 else
7174 dst_type = ADDR_LE_DEV_RANDOM;
7175
7176 if (hci_dev_test_flag(hdev, HCI_ADVERTISING))
7177 hcon = hci_connect_le(hdev, dst, dst_type,
7178 chan->sec_level,
7179 HCI_LE_CONN_TIMEOUT,
7180 HCI_ROLE_SLAVE, NULL);
7181 else
7182 hcon = hci_connect_le_scan(hdev, dst, dst_type,
7183 chan->sec_level,
7184 HCI_LE_CONN_TIMEOUT);
7185
7186 } else {
7187 u8 auth_type = l2cap_get_auth_type(chan);
7188 hcon = hci_connect_acl(hdev, dst, chan->sec_level, auth_type);
7189 }
7190
7191 if (IS_ERR(hcon)) {
7192 err = PTR_ERR(hcon);
7193 goto done;
7194 }
7195
7196 conn = l2cap_conn_add(hcon);
7197 if (!conn) {
7198 hci_conn_drop(hcon);
7199 err = -ENOMEM;
7200 goto done;
7201 }
7202
7203 mutex_lock(&conn->chan_lock);
7204 l2cap_chan_lock(chan);
7205
7206 if (cid && __l2cap_get_chan_by_dcid(conn, cid)) {
7207 hci_conn_drop(hcon);
7208 err = -EBUSY;
7209 goto chan_unlock;
7210 }
7211
7212 /* Update source addr of the socket */
7213 bacpy(&chan->src, &hcon->src);
7214 chan->src_type = bdaddr_src_type(hcon);
7215
7216 __l2cap_chan_add(conn, chan);
7217
7218 /* l2cap_chan_add takes its own ref so we can drop this one */
7219 hci_conn_drop(hcon);
7220
7221 l2cap_state_change(chan, BT_CONNECT);
7222 __set_chan_timer(chan, chan->ops->get_sndtimeo(chan));
7223
7224 /* Release chan->sport so that it can be reused by other
7225 * sockets (as it's only used for listening sockets).
7226 */
7227 write_lock(&chan_list_lock);
7228 chan->sport = 0;
7229 write_unlock(&chan_list_lock);
7230
7231 if (hcon->state == BT_CONNECTED) {
7232 if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
7233 __clear_chan_timer(chan);
7234 if (l2cap_chan_check_security(chan, true))
7235 l2cap_state_change(chan, BT_CONNECTED);
7236 } else
7237 l2cap_do_start(chan);
7238 }
7239
7240 err = 0;
7241
7242 chan_unlock:
7243 l2cap_chan_unlock(chan);
7244 mutex_unlock(&conn->chan_lock);
7245 done:
7246 hci_dev_unlock(hdev);
7247 hci_dev_put(hdev);
7248 return err;
7249 }
7250 EXPORT_SYMBOL_GPL(l2cap_chan_connect);
7251
7252 /* ---- L2CAP interface with lower layer (HCI) ---- */
7253
7254 int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr)
7255 {
7256 int exact = 0, lm1 = 0, lm2 = 0;
7257 struct l2cap_chan *c;
7258
7259 BT_DBG("hdev %s, bdaddr %pMR", hdev->name, bdaddr);
7260
7261 /* Find listening sockets and check their link_mode */
7262 read_lock(&chan_list_lock);
7263 list_for_each_entry(c, &chan_list, global_l) {
7264 if (c->state != BT_LISTEN)
7265 continue;
7266
7267 if (!bacmp(&c->src, &hdev->bdaddr)) {
7268 lm1 |= HCI_LM_ACCEPT;
7269 if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
7270 lm1 |= HCI_LM_MASTER;
7271 exact++;
7272 } else if (!bacmp(&c->src, BDADDR_ANY)) {
7273 lm2 |= HCI_LM_ACCEPT;
7274 if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
7275 lm2 |= HCI_LM_MASTER;
7276 }
7277 }
7278 read_unlock(&chan_list_lock);
7279
7280 return exact ? lm1 : lm2;
7281 }
7282
7283 /* Find the next fixed channel in BT_LISTEN state, continue iteration
7284 * from an existing channel in the list or from the beginning of the
7285 * global list (by passing NULL as first parameter).
7286 */
7287 static struct l2cap_chan *l2cap_global_fixed_chan(struct l2cap_chan *c,
7288 struct hci_conn *hcon)
7289 {
7290 u8 src_type = bdaddr_src_type(hcon);
7291
7292 read_lock(&chan_list_lock);
7293
7294 if (c)
7295 c = list_next_entry(c, global_l);
7296 else
7297 c = list_entry(chan_list.next, typeof(*c), global_l);
7298
7299 list_for_each_entry_from(c, &chan_list, global_l) {
7300 if (c->chan_type != L2CAP_CHAN_FIXED)
7301 continue;
7302 if (c->state != BT_LISTEN)
7303 continue;
7304 if (bacmp(&c->src, &hcon->src) && bacmp(&c->src, BDADDR_ANY))
7305 continue;
7306 if (src_type != c->src_type)
7307 continue;
7308
7309 l2cap_chan_hold(c);
7310 read_unlock(&chan_list_lock);
7311 return c;
7312 }
7313
7314 read_unlock(&chan_list_lock);
7315
7316 return NULL;
7317 }
7318
7319 static void l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
7320 {
7321 struct hci_dev *hdev = hcon->hdev;
7322 struct l2cap_conn *conn;
7323 struct l2cap_chan *pchan;
7324 u8 dst_type;
7325
7326 if (hcon->type != ACL_LINK && hcon->type != LE_LINK)
7327 return;
7328
7329 BT_DBG("hcon %p bdaddr %pMR status %d", hcon, &hcon->dst, status);
7330
7331 if (status) {
7332 l2cap_conn_del(hcon, bt_to_errno(status));
7333 return;
7334 }
7335
7336 conn = l2cap_conn_add(hcon);
7337 if (!conn)
7338 return;
7339
7340 dst_type = bdaddr_dst_type(hcon);
7341
7342 /* If device is blocked, do not create channels for it */
7343 if (hci_bdaddr_list_lookup(&hdev->blacklist, &hcon->dst, dst_type))
7344 return;
7345
7346 /* Find fixed channels and notify them of the new connection. We
7347 * use multiple individual lookups, continuing each time where
7348 * we left off, because the list lock would prevent calling the
7349 * potentially sleeping l2cap_chan_lock() function.
7350 */
7351 pchan = l2cap_global_fixed_chan(NULL, hcon);
7352 while (pchan) {
7353 struct l2cap_chan *chan, *next;
7354
7355 /* Client fixed channels should override server ones */
7356 if (__l2cap_get_chan_by_dcid(conn, pchan->scid))
7357 goto next;
7358
7359 l2cap_chan_lock(pchan);
7360 chan = pchan->ops->new_connection(pchan);
7361 if (chan) {
7362 bacpy(&chan->src, &hcon->src);
7363 bacpy(&chan->dst, &hcon->dst);
7364 chan->src_type = bdaddr_src_type(hcon);
7365 chan->dst_type = dst_type;
7366
7367 __l2cap_chan_add(conn, chan);
7368 }
7369
7370 l2cap_chan_unlock(pchan);
7371 next:
7372 next = l2cap_global_fixed_chan(pchan, hcon);
7373 l2cap_chan_put(pchan);
7374 pchan = next;
7375 }
7376
7377 l2cap_conn_ready(conn);
7378 }
7379
7380 int l2cap_disconn_ind(struct hci_conn *hcon)
7381 {
7382 struct l2cap_conn *conn = hcon->l2cap_data;
7383
7384 BT_DBG("hcon %p", hcon);
7385
7386 if (!conn)
7387 return HCI_ERROR_REMOTE_USER_TERM;
7388 return conn->disc_reason;
7389 }
7390
7391 static void l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
7392 {
7393 if (hcon->type != ACL_LINK && hcon->type != LE_LINK)
7394 return;
7395
7396 BT_DBG("hcon %p reason %d", hcon, reason);
7397
7398 l2cap_conn_del(hcon, bt_to_errno(reason));
7399 }
7400
7401 static inline void l2cap_check_encryption(struct l2cap_chan *chan, u8 encrypt)
7402 {
7403 if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED)
7404 return;
7405
7406 if (encrypt == 0x00) {
7407 if (chan->sec_level == BT_SECURITY_MEDIUM) {
7408 __set_chan_timer(chan, L2CAP_ENC_TIMEOUT);
7409 } else if (chan->sec_level == BT_SECURITY_HIGH ||
7410 chan->sec_level == BT_SECURITY_FIPS)
7411 l2cap_chan_close(chan, ECONNREFUSED);
7412 } else {
7413 if (chan->sec_level == BT_SECURITY_MEDIUM)
7414 __clear_chan_timer(chan);
7415 }
7416 }
7417
7418 static void l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
7419 {
7420 struct l2cap_conn *conn = hcon->l2cap_data;
7421 struct l2cap_chan *chan;
7422
7423 if (!conn)
7424 return;
7425
7426 BT_DBG("conn %p status 0x%2.2x encrypt %u", conn, status, encrypt);
7427
7428 mutex_lock(&conn->chan_lock);
7429
7430 list_for_each_entry(chan, &conn->chan_l, list) {
7431 l2cap_chan_lock(chan);
7432
7433 BT_DBG("chan %p scid 0x%4.4x state %s", chan, chan->scid,
7434 state_to_string(chan->state));
7435
7436 if (chan->scid == L2CAP_CID_A2MP) {
7437 l2cap_chan_unlock(chan);
7438 continue;
7439 }
7440
7441 if (!status && encrypt)
7442 chan->sec_level = hcon->sec_level;
7443
7444 if (!__l2cap_no_conn_pending(chan)) {
7445 l2cap_chan_unlock(chan);
7446 continue;
7447 }
7448
7449 if (!status && (chan->state == BT_CONNECTED ||
7450 chan->state == BT_CONFIG)) {
7451 chan->ops->resume(chan);
7452 l2cap_check_encryption(chan, encrypt);
7453 l2cap_chan_unlock(chan);
7454 continue;
7455 }
7456
7457 if (chan->state == BT_CONNECT) {
7458 if (!status)
7459 l2cap_start_connection(chan);
7460 else
7461 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
7462 } else if (chan->state == BT_CONNECT2 &&
7463 chan->mode != L2CAP_MODE_LE_FLOWCTL) {
7464 struct l2cap_conn_rsp rsp;
7465 __u16 res, stat;
7466
7467 if (!status) {
7468 if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
7469 res = L2CAP_CR_PEND;
7470 stat = L2CAP_CS_AUTHOR_PEND;
7471 chan->ops->defer(chan);
7472 } else {
7473 l2cap_state_change(chan, BT_CONFIG);
7474 res = L2CAP_CR_SUCCESS;
7475 stat = L2CAP_CS_NO_INFO;
7476 }
7477 } else {
7478 l2cap_state_change(chan, BT_DISCONN);
7479 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
7480 res = L2CAP_CR_SEC_BLOCK;
7481 stat = L2CAP_CS_NO_INFO;
7482 }
7483
7484 rsp.scid = cpu_to_le16(chan->dcid);
7485 rsp.dcid = cpu_to_le16(chan->scid);
7486 rsp.result = cpu_to_le16(res);
7487 rsp.status = cpu_to_le16(stat);
7488 l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
7489 sizeof(rsp), &rsp);
7490
7491 if (!test_bit(CONF_REQ_SENT, &chan->conf_state) &&
7492 res == L2CAP_CR_SUCCESS) {
7493 char buf[128];
7494 set_bit(CONF_REQ_SENT, &chan->conf_state);
7495 l2cap_send_cmd(conn, l2cap_get_ident(conn),
7496 L2CAP_CONF_REQ,
7497 l2cap_build_conf_req(chan, buf, sizeof(buf)),
7498 buf);
7499 chan->num_conf_req++;
7500 }
7501 }
7502
7503 l2cap_chan_unlock(chan);
7504 }
7505
7506 mutex_unlock(&conn->chan_lock);
7507 }
7508
7509 void l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
7510 {
7511 struct l2cap_conn *conn = hcon->l2cap_data;
7512 struct l2cap_hdr *hdr;
7513 int len;
7514
7515 /* For AMP controller do not create l2cap conn */
7516 if (!conn && hcon->hdev->dev_type != HCI_PRIMARY)
7517 goto drop;
7518
7519 if (!conn)
7520 conn = l2cap_conn_add(hcon);
7521
7522 if (!conn)
7523 goto drop;
7524
7525 BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
7526
7527 switch (flags) {
7528 case ACL_START:
7529 case ACL_START_NO_FLUSH:
7530 case ACL_COMPLETE:
7531 if (conn->rx_len) {
7532 BT_ERR("Unexpected start frame (len %d)", skb->len);
7533 kfree_skb(conn->rx_skb);
7534 conn->rx_skb = NULL;
7535 conn->rx_len = 0;
7536 l2cap_conn_unreliable(conn, ECOMM);
7537 }
7538
7539 /* Start fragment always begin with Basic L2CAP header */
7540 if (skb->len < L2CAP_HDR_SIZE) {
7541 BT_ERR("Frame is too short (len %d)", skb->len);
7542 l2cap_conn_unreliable(conn, ECOMM);
7543 goto drop;
7544 }
7545
7546 hdr = (struct l2cap_hdr *) skb->data;
7547 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
7548
7549 if (len == skb->len) {
7550 /* Complete frame received */
7551 l2cap_recv_frame(conn, skb);
7552 return;
7553 }
7554
7555 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
7556
7557 if (skb->len > len) {
7558 BT_ERR("Frame is too long (len %d, expected len %d)",
7559 skb->len, len);
7560 l2cap_conn_unreliable(conn, ECOMM);
7561 goto drop;
7562 }
7563
7564 /* Allocate skb for the complete frame (with header) */
7565 conn->rx_skb = bt_skb_alloc(len, GFP_KERNEL);
7566 if (!conn->rx_skb)
7567 goto drop;
7568
7569 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
7570 skb->len);
7571 conn->rx_len = len - skb->len;
7572 break;
7573
7574 case ACL_CONT:
7575 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
7576
7577 if (!conn->rx_len) {
7578 BT_ERR("Unexpected continuation frame (len %d)", skb->len);
7579 l2cap_conn_unreliable(conn, ECOMM);
7580 goto drop;
7581 }
7582
7583 if (skb->len > conn->rx_len) {
7584 BT_ERR("Fragment is too long (len %d, expected %d)",
7585 skb->len, conn->rx_len);
7586 kfree_skb(conn->rx_skb);
7587 conn->rx_skb = NULL;
7588 conn->rx_len = 0;
7589 l2cap_conn_unreliable(conn, ECOMM);
7590 goto drop;
7591 }
7592
7593 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
7594 skb->len);
7595 conn->rx_len -= skb->len;
7596
7597 if (!conn->rx_len) {
7598 /* Complete frame received. l2cap_recv_frame
7599 * takes ownership of the skb so set the global
7600 * rx_skb pointer to NULL first.
7601 */
7602 struct sk_buff *rx_skb = conn->rx_skb;
7603 conn->rx_skb = NULL;
7604 l2cap_recv_frame(conn, rx_skb);
7605 }
7606 break;
7607 }
7608
7609 drop:
7610 kfree_skb(skb);
7611 }
7612
7613 static struct hci_cb l2cap_cb = {
7614 .name = "L2CAP",
7615 .connect_cfm = l2cap_connect_cfm,
7616 .disconn_cfm = l2cap_disconn_cfm,
7617 .security_cfm = l2cap_security_cfm,
7618 };
7619
7620 static int l2cap_debugfs_show(struct seq_file *f, void *p)
7621 {
7622 struct l2cap_chan *c;
7623
7624 read_lock(&chan_list_lock);
7625
7626 list_for_each_entry(c, &chan_list, global_l) {
7627 seq_printf(f, "%pMR (%u) %pMR (%u) %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n",
7628 &c->src, c->src_type, &c->dst, c->dst_type,
7629 c->state, __le16_to_cpu(c->psm),
7630 c->scid, c->dcid, c->imtu, c->omtu,
7631 c->sec_level, c->mode);
7632 }
7633
7634 read_unlock(&chan_list_lock);
7635
7636 return 0;
7637 }
7638
7639 static int l2cap_debugfs_open(struct inode *inode, struct file *file)
7640 {
7641 return single_open(file, l2cap_debugfs_show, inode->i_private);
7642 }
7643
7644 static const struct file_operations l2cap_debugfs_fops = {
7645 .open = l2cap_debugfs_open,
7646 .read = seq_read,
7647 .llseek = seq_lseek,
7648 .release = single_release,
7649 };
7650
7651 static struct dentry *l2cap_debugfs;
7652
7653 int __init l2cap_init(void)
7654 {
7655 int err;
7656
7657 err = l2cap_init_sockets();
7658 if (err < 0)
7659 return err;
7660
7661 hci_register_cb(&l2cap_cb);
7662
7663 if (IS_ERR_OR_NULL(bt_debugfs))
7664 return 0;
7665
7666 l2cap_debugfs = debugfs_create_file("l2cap", 0444, bt_debugfs,
7667 NULL, &l2cap_debugfs_fops);
7668
7669 debugfs_create_u16("l2cap_le_max_credits", 0644, bt_debugfs,
7670 &le_max_credits);
7671 debugfs_create_u16("l2cap_le_default_mps", 0644, bt_debugfs,
7672 &le_default_mps);
7673
7674 return 0;
7675 }
7676
7677 void l2cap_exit(void)
7678 {
7679 debugfs_remove(l2cap_debugfs);
7680 hci_unregister_cb(&l2cap_cb);
7681 l2cap_cleanup_sockets();
7682 }
7683
7684 module_param(disable_ertm, bool, 0644);
7685 MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode");
Linux with added FPC-III support