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