Kernel

Kernel Git Source Tree

Root/net/bluetooth/l2cap_core.c

1	/*
2	BlueZ - Bluetooth protocol stack for Linux
3	Copyright (C) 2000-2001 Qualcomm Incorporated
4	Copyright (C) 2009-2010 Gustavo F. Padovan <gustavo@padovan.org>
5	Copyright (C) 2010 Google Inc.
6	Copyright (C) 2011 ProFUSION Embedded Systems
7	Copyright (c) 2012 Code Aurora Forum. All rights reserved.
8
9	Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.com>
10
11	This program is free software; you can redistribute it and/or modify
12	it under the terms of the GNU General Public License version 2 as
13	published by the Free Software Foundation;
14
15	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
16	OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS.
18	IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY
19	CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES
20	WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
21	ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
22	OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
23
24	ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS,
25	COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS
26	SOFTWARE IS DISCLAIMED.
27	*/
28
29	/* Bluetooth L2CAP core. */
30
31	#include <linux/module.h>
32
33	#include <linux/debugfs.h>
34	#include <linux/crc16.h>
35
36	#include <net/bluetooth/bluetooth.h>
37	#include <net/bluetooth/hci_core.h>
38	#include <net/bluetooth/l2cap.h>
39
40	#include "smp.h"
41	#include "a2mp.h"
42	#include "amp.h"
43
44	bool disable_ertm;
45
46	static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN \| L2CAP_FEAT_UCD;
47	static u8 l2cap_fixed_chan[8] = { L2CAP_FC_L2CAP \| L2CAP_FC_CONNLESS, };
48
49	static LIST_HEAD(chan_list);
50	static DEFINE_RWLOCK(chan_list_lock);
51
52	static struct sk_buff l2cap_build_cmd(struct l2cap_conn conn,
53	u8 code, u8 ident, u16 dlen, void *data);
54	static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len,
55	void *data);
56	static int l2cap_build_conf_req(struct l2cap_chan chan, void data);
57	static void l2cap_send_disconn_req(struct l2cap_chan *chan, int err);
58
59	static void l2cap_tx(struct l2cap_chan chan, struct l2cap_ctrl control,
60	struct sk_buff_head *skbs, u8 event);
61
62	static inline __u8 bdaddr_type(struct hci_conn *hcon, __u8 type)
63	{
64	if (hcon->type == LE_LINK) {
65	if (type == ADDR_LE_DEV_PUBLIC)
66	return BDADDR_LE_PUBLIC;
67	else
68	return BDADDR_LE_RANDOM;
69	}
70
71	return BDADDR_BREDR;
72	}
73
74	/* ---- L2CAP channels ---- */
75
76	static struct l2cap_chan __l2cap_get_chan_by_dcid(struct l2cap_conn conn,
77	u16 cid)
78	{
79	struct l2cap_chan *c;
80
81	list_for_each_entry(c, &conn->chan_l, list) {
82	if (c->dcid == cid)
83	return c;
84	}
85	return NULL;
86	}
87
88	static struct l2cap_chan __l2cap_get_chan_by_scid(struct l2cap_conn conn,
89	u16 cid)
90	{
91	struct l2cap_chan *c;
92
93	list_for_each_entry(c, &conn->chan_l, list) {
94	if (c->scid == cid)
95	return c;
96	}
97	return NULL;
98	}
99
100	/* Find channel with given SCID.
101	* Returns locked channel. */
102	static struct l2cap_chan l2cap_get_chan_by_scid(struct l2cap_conn conn,
103	u16 cid)
104	{
105	struct l2cap_chan *c;
106
107	mutex_lock(&conn->chan_lock);
108	c = __l2cap_get_chan_by_scid(conn, cid);
109	if (c)
110	l2cap_chan_lock(c);
111	mutex_unlock(&conn->chan_lock);
112
113	return c;
114	}
115
116	/* Find channel with given DCID.
117	* Returns locked channel.
118	*/
119	static struct l2cap_chan l2cap_get_chan_by_dcid(struct l2cap_conn conn,
120	u16 cid)
121	{
122	struct l2cap_chan *c;
123
124	mutex_lock(&conn->chan_lock);
125	c = __l2cap_get_chan_by_dcid(conn, cid);
126	if (c)
127	l2cap_chan_lock(c);
128	mutex_unlock(&conn->chan_lock);
129
130	return c;
131	}
132
133	static struct l2cap_chan __l2cap_get_chan_by_ident(struct l2cap_conn conn,
134	u8 ident)
135	{
136	struct l2cap_chan *c;
137
138	list_for_each_entry(c, &conn->chan_l, list) {
139	if (c->ident == ident)
140	return c;
141	}
142	return NULL;
143	}
144
145	static struct l2cap_chan l2cap_get_chan_by_ident(struct l2cap_conn conn,
146	u8 ident)
147	{
148	struct l2cap_chan *c;
149
150	mutex_lock(&conn->chan_lock);
151	c = __l2cap_get_chan_by_ident(conn, ident);
152	if (c)
153	l2cap_chan_lock(c);
154	mutex_unlock(&conn->chan_lock);
155
156	return c;
157	}
158
159	static struct l2cap_chan __l2cap_global_chan_by_addr(__le16 psm, bdaddr_t src)
160	{
161	struct l2cap_chan *c;
162
163	list_for_each_entry(c, &chan_list, global_l) {
164	if (c->sport == psm && !bacmp(&c->src, src))
165	return c;
166	}
167	return NULL;
168	}
169
170	int l2cap_add_psm(struct l2cap_chan chan, bdaddr_t src, __le16 psm)
171	{
172	int err;
173
174	write_lock(&chan_list_lock);
175
176	if (psm && __l2cap_global_chan_by_addr(psm, src)) {
177	err = -EADDRINUSE;
178	goto done;
179	}
180
181	if (psm) {
182	chan->psm = psm;
183	chan->sport = psm;
184	err = 0;
185	} else {
186	u16 p;
187
188	err = -EINVAL;
189	for (p = 0x1001; p < 0x1100; p += 2)
190	if (!__l2cap_global_chan_by_addr(cpu_to_le16(p), src)) {
191	chan->psm = cpu_to_le16(p);
192	chan->sport = cpu_to_le16(p);
193	err = 0;
194	break;
195	}
196	}
197
198	done:
199	write_unlock(&chan_list_lock);
200	return err;
201	}
202
203	int l2cap_add_scid(struct l2cap_chan *chan, __u16 scid)
204	{
205	write_lock(&chan_list_lock);
206
207	chan->scid = scid;
208
209	write_unlock(&chan_list_lock);
210
211	return 0;
212	}
213
214	static u16 l2cap_alloc_cid(struct l2cap_conn *conn)
215	{
216	u16 cid = L2CAP_CID_DYN_START;
217
218	for (; cid < L2CAP_CID_DYN_END; cid++) {
219	if (!__l2cap_get_chan_by_scid(conn, cid))
220	return cid;
221	}
222
223	return 0;
224	}
225
226	static void l2cap_state_change(struct l2cap_chan *chan, int state)
227	{
228	BT_DBG("chan %p %s -> %s", chan, state_to_string(chan->state),
229	state_to_string(state));
230
231	chan->state = state;
232	chan->ops->state_change(chan, state, 0);
233	}
234
235	static inline void l2cap_state_change_and_error(struct l2cap_chan *chan,
236	int state, int err)
237	{
238	chan->state = state;
239	chan->ops->state_change(chan, chan->state, err);
240	}
241
242	static inline void l2cap_chan_set_err(struct l2cap_chan *chan, int err)
243	{
244	chan->ops->state_change(chan, chan->state, err);
245	}
246
247	static void __set_retrans_timer(struct l2cap_chan *chan)
248	{
249	if (!delayed_work_pending(&chan->monitor_timer) &&
250	chan->retrans_timeout) {
251	l2cap_set_timer(chan, &chan->retrans_timer,
252	msecs_to_jiffies(chan->retrans_timeout));
253	}
254	}
255
256	static void __set_monitor_timer(struct l2cap_chan *chan)
257	{
258	__clear_retrans_timer(chan);
259	if (chan->monitor_timeout) {
260	l2cap_set_timer(chan, &chan->monitor_timer,
261	msecs_to_jiffies(chan->monitor_timeout));
262	}
263	}
264
265	static struct sk_buff l2cap_ertm_seq_in_queue(struct sk_buff_head head,
266	u16 seq)
267	{
268	struct sk_buff *skb;
269
270	skb_queue_walk(head, skb) {
271	if (bt_cb(skb)->control.txseq == seq)
272	return skb;
273	}
274
275	return NULL;
276	}
277
278	/* ---- L2CAP sequence number lists ---- */
279
280	/* For ERTM, ordered lists of sequence numbers must be tracked for
281	* SREJ requests that are received and for frames that are to be
282	* retransmitted. These seq_list functions implement a singly-linked
283	* list in an array, where membership in the list can also be checked
284	* in constant time. Items can also be added to the tail of the list
285	* and removed from the head in constant time, without further memory
286	* allocs or frees.
287	*/
288
289	static int l2cap_seq_list_init(struct l2cap_seq_list *seq_list, u16 size)
290	{
291	size_t alloc_size, i;
292
293	/* Allocated size is a power of 2 to map sequence numbers
294	* (which may be up to 14 bits) in to a smaller array that is
295	* sized for the negotiated ERTM transmit windows.
296	*/
297	alloc_size = roundup_pow_of_two(size);
298
299	seq_list->list = kmalloc(sizeof(u16) * alloc_size, GFP_KERNEL);
300	if (!seq_list->list)
301	return -ENOMEM;
302
303	seq_list->mask = alloc_size - 1;
304	seq_list->head = L2CAP_SEQ_LIST_CLEAR;
305	seq_list->tail = L2CAP_SEQ_LIST_CLEAR;
306	for (i = 0; i < alloc_size; i++)
307	seq_list->list[i] = L2CAP_SEQ_LIST_CLEAR;
308
309	return 0;
310	}
311
312	static inline void l2cap_seq_list_free(struct l2cap_seq_list *seq_list)
313	{
314	kfree(seq_list->list);
315	}
316
317	static inline bool l2cap_seq_list_contains(struct l2cap_seq_list *seq_list,
318	u16 seq)
319	{
320	/* Constant-time check for list membership */
321	return seq_list->list[seq & seq_list->mask] != L2CAP_SEQ_LIST_CLEAR;
322	}
323
324	static u16 l2cap_seq_list_remove(struct l2cap_seq_list *seq_list, u16 seq)
325	{
326	u16 mask = seq_list->mask;
327
328	if (seq_list->head == L2CAP_SEQ_LIST_CLEAR) {
329	/* In case someone tries to pop the head of an empty list */
330	return L2CAP_SEQ_LIST_CLEAR;
331	} else if (seq_list->head == seq) {
332	/* Head can be removed in constant time */
333	seq_list->head = seq_list->list[seq & mask];
334	seq_list->list[seq & mask] = L2CAP_SEQ_LIST_CLEAR;
335
336	if (seq_list->head == L2CAP_SEQ_LIST_TAIL) {
337	seq_list->head = L2CAP_SEQ_LIST_CLEAR;
338	seq_list->tail = L2CAP_SEQ_LIST_CLEAR;
339	}
340	} else {
341	/* Walk the list to find the sequence number */
342	u16 prev = seq_list->head;
343	while (seq_list->list[prev & mask] != seq) {
344	prev = seq_list->list[prev & mask];
345	if (prev == L2CAP_SEQ_LIST_TAIL)
346	return L2CAP_SEQ_LIST_CLEAR;
347	}
348
349	/* Unlink the number from the list and clear it */
350	seq_list->list[prev & mask] = seq_list->list[seq & mask];
351	seq_list->list[seq & mask] = L2CAP_SEQ_LIST_CLEAR;
352	if (seq_list->tail == seq)
353	seq_list->tail = prev;
354	}
355	return seq;
356	}
357
358	static inline u16 l2cap_seq_list_pop(struct l2cap_seq_list *seq_list)
359	{
360	/* Remove the head in constant time */
361	return l2cap_seq_list_remove(seq_list, seq_list->head);
362	}
363
364	static void l2cap_seq_list_clear(struct l2cap_seq_list *seq_list)
365	{
366	u16 i;
367
368	if (seq_list->head == L2CAP_SEQ_LIST_CLEAR)
369	return;
370
371	for (i = 0; i <= seq_list->mask; i++)
372	seq_list->list[i] = L2CAP_SEQ_LIST_CLEAR;
373
374	seq_list->head = L2CAP_SEQ_LIST_CLEAR;
375	seq_list->tail = L2CAP_SEQ_LIST_CLEAR;
376	}
377
378	static void l2cap_seq_list_append(struct l2cap_seq_list *seq_list, u16 seq)
379	{
380	u16 mask = seq_list->mask;
381
382	/* All appends happen in constant time */
383
384	if (seq_list->list[seq & mask] != L2CAP_SEQ_LIST_CLEAR)
385	return;
386
387	if (seq_list->tail == L2CAP_SEQ_LIST_CLEAR)
388	seq_list->head = seq;
389	else
390	seq_list->list[seq_list->tail & mask] = seq;
391
392	seq_list->tail = seq;
393	seq_list->list[seq & mask] = L2CAP_SEQ_LIST_TAIL;
394	}
395
396	static void l2cap_chan_timeout(struct work_struct *work)
397	{
398	struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
399	chan_timer.work);
400	struct l2cap_conn *conn = chan->conn;
401	int reason;
402
403	BT_DBG("chan %p state %s", chan, state_to_string(chan->state));
404
405	mutex_lock(&conn->chan_lock);
406	l2cap_chan_lock(chan);
407
408	if (chan->state == BT_CONNECTED \|\| chan->state == BT_CONFIG)
409	reason = ECONNREFUSED;
410	else if (chan->state == BT_CONNECT &&
411	chan->sec_level != BT_SECURITY_SDP)
412	reason = ECONNREFUSED;
413	else
414	reason = ETIMEDOUT;
415
416	l2cap_chan_close(chan, reason);
417
418	l2cap_chan_unlock(chan);
419
420	chan->ops->close(chan);
421	mutex_unlock(&conn->chan_lock);
422
423	l2cap_chan_put(chan);
424	}
425
426	struct l2cap_chan *l2cap_chan_create(void)
427	{
428	struct l2cap_chan *chan;
429
430	chan = kzalloc(sizeof(*chan), GFP_ATOMIC);
431	if (!chan)
432	return NULL;
433
434	mutex_init(&chan->lock);
435
436	write_lock(&chan_list_lock);
437	list_add(&chan->global_l, &chan_list);
438	write_unlock(&chan_list_lock);
439
440	INIT_DELAYED_WORK(&chan->chan_timer, l2cap_chan_timeout);
441
442	chan->state = BT_OPEN;
443
444	kref_init(&chan->kref);
445
446	/* This flag is cleared in l2cap_chan_ready() */
447	set_bit(CONF_NOT_COMPLETE, &chan->conf_state);
448
449	BT_DBG("chan %p", chan);
450
451	return chan;
452	}
453
454	static void l2cap_chan_destroy(struct kref *kref)
455	{
456	struct l2cap_chan *chan = container_of(kref, struct l2cap_chan, kref);
457
458	BT_DBG("chan %p", chan);
459
460	write_lock(&chan_list_lock);
461	list_del(&chan->global_l);
462	write_unlock(&chan_list_lock);
463
464	kfree(chan);
465	}
466
467	void l2cap_chan_hold(struct l2cap_chan *c)
468	{
469	BT_DBG("chan %p orig refcnt %d", c, atomic_read(&c->kref.refcount));
470
471	kref_get(&c->kref);
472	}
473
474	void l2cap_chan_put(struct l2cap_chan *c)
475	{
476	BT_DBG("chan %p orig refcnt %d", c, atomic_read(&c->kref.refcount));
477
478	kref_put(&c->kref, l2cap_chan_destroy);
479	}
480
481	void l2cap_chan_set_defaults(struct l2cap_chan *chan)
482	{
483	chan->fcs = L2CAP_FCS_CRC16;
484	chan->max_tx = L2CAP_DEFAULT_MAX_TX;
485	chan->tx_win = L2CAP_DEFAULT_TX_WINDOW;
486	chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW;
487	chan->ack_win = L2CAP_DEFAULT_TX_WINDOW;
488	chan->sec_level = BT_SECURITY_LOW;
489
490	set_bit(FLAG_FORCE_ACTIVE, &chan->flags);
491	}
492
493	void __l2cap_chan_add(struct l2cap_conn conn, struct l2cap_chan chan)
494	{
495	BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn,
496	__le16_to_cpu(chan->psm), chan->dcid);
497
498	conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM;
499
500	chan->conn = conn;
501
502	switch (chan->chan_type) {
503	case L2CAP_CHAN_CONN_ORIENTED:
504	if (conn->hcon->type == LE_LINK) {
505	/* LE connection */
506	chan->omtu = L2CAP_DEFAULT_MTU;
507	if (chan->dcid == L2CAP_CID_ATT)
508	chan->scid = L2CAP_CID_ATT;
509	else
510	chan->scid = l2cap_alloc_cid(conn);
511	} else {
512	/* Alloc CID for connection-oriented socket */
513	chan->scid = l2cap_alloc_cid(conn);
514	chan->omtu = L2CAP_DEFAULT_MTU;
515	}
516	break;
517
518	case L2CAP_CHAN_CONN_LESS:
519	/* Connectionless socket */
520	chan->scid = L2CAP_CID_CONN_LESS;
521	chan->dcid = L2CAP_CID_CONN_LESS;
522	chan->omtu = L2CAP_DEFAULT_MTU;
523	break;
524
525	case L2CAP_CHAN_CONN_FIX_A2MP:
526	chan->scid = L2CAP_CID_A2MP;
527	chan->dcid = L2CAP_CID_A2MP;
528	chan->omtu = L2CAP_A2MP_DEFAULT_MTU;
529	chan->imtu = L2CAP_A2MP_DEFAULT_MTU;
530	break;
531
532	default:
533	/* Raw socket can send/recv signalling messages only */
534	chan->scid = L2CAP_CID_SIGNALING;
535	chan->dcid = L2CAP_CID_SIGNALING;
536	chan->omtu = L2CAP_DEFAULT_MTU;
537	}
538
539	chan->local_id = L2CAP_BESTEFFORT_ID;
540	chan->local_stype = L2CAP_SERV_BESTEFFORT;
541	chan->local_msdu = L2CAP_DEFAULT_MAX_SDU_SIZE;
542	chan->local_sdu_itime = L2CAP_DEFAULT_SDU_ITIME;
543	chan->local_acc_lat = L2CAP_DEFAULT_ACC_LAT;
544	chan->local_flush_to = L2CAP_EFS_DEFAULT_FLUSH_TO;
545
546	l2cap_chan_hold(chan);
547
548	hci_conn_hold(conn->hcon);
549
550	list_add(&chan->list, &conn->chan_l);
551	}
552
553	void l2cap_chan_add(struct l2cap_conn conn, struct l2cap_chan chan)
554	{
555	mutex_lock(&conn->chan_lock);
556	__l2cap_chan_add(conn, chan);
557	mutex_unlock(&conn->chan_lock);
558	}
559
560	void l2cap_chan_del(struct l2cap_chan *chan, int err)
561	{
562	struct l2cap_conn *conn = chan->conn;
563
564	__clear_chan_timer(chan);
565
566	BT_DBG("chan %p, conn %p, err %d", chan, conn, err);
567
568	if (conn) {
569	struct amp_mgr *mgr = conn->hcon->amp_mgr;
570	/* Delete from channel list */
571	list_del(&chan->list);
572
573	l2cap_chan_put(chan);
574
575	chan->conn = NULL;
576
577	if (chan->chan_type != L2CAP_CHAN_CONN_FIX_A2MP)
578	hci_conn_drop(conn->hcon);
579
580	if (mgr && mgr->bredr_chan == chan)
581	mgr->bredr_chan = NULL;
582	}
583
584	if (chan->hs_hchan) {
585	struct hci_chan *hs_hchan = chan->hs_hchan;
586
587	BT_DBG("chan %p disconnect hs_hchan %p", chan, hs_hchan);
588	amp_disconnect_logical_link(hs_hchan);
589	}
590
591	chan->ops->teardown(chan, err);
592
593	if (test_bit(CONF_NOT_COMPLETE, &chan->conf_state))
594	return;
595
596	switch(chan->mode) {
597	case L2CAP_MODE_BASIC:
598	break;
599
600	case L2CAP_MODE_ERTM:
601	__clear_retrans_timer(chan);
602	__clear_monitor_timer(chan);
603	__clear_ack_timer(chan);
604
605	skb_queue_purge(&chan->srej_q);
606
607	l2cap_seq_list_free(&chan->srej_list);
608	l2cap_seq_list_free(&chan->retrans_list);
609
610	/* fall through */
611
612	case L2CAP_MODE_STREAMING:
613	skb_queue_purge(&chan->tx_q);
614	break;
615	}
616
617	return;
618	}
619
620	void l2cap_chan_close(struct l2cap_chan *chan, int reason)
621	{
622	struct l2cap_conn *conn = chan->conn;
623
624	BT_DBG("chan %p state %s", chan, state_to_string(chan->state));
625
626	switch (chan->state) {
627	case BT_LISTEN:
628	chan->ops->teardown(chan, 0);
629	break;
630
631	case BT_CONNECTED:
632	case BT_CONFIG:
633	if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED &&
634	conn->hcon->type == ACL_LINK) {
635	__set_chan_timer(chan, chan->ops->get_sndtimeo(chan));
636	l2cap_send_disconn_req(chan, reason);
637	} else
638	l2cap_chan_del(chan, reason);
639	break;
640
641	case BT_CONNECT2:
642	if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED &&
643	conn->hcon->type == ACL_LINK) {
644	struct l2cap_conn_rsp rsp;
645	__u16 result;
646
647	if (test_bit(FLAG_DEFER_SETUP, &chan->flags))
648	result = L2CAP_CR_SEC_BLOCK;
649	else
650	result = L2CAP_CR_BAD_PSM;
651
652	l2cap_state_change(chan, BT_DISCONN);
653
654	rsp.scid = cpu_to_le16(chan->dcid);
655	rsp.dcid = cpu_to_le16(chan->scid);
656	rsp.result = cpu_to_le16(result);
657	rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
658	l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
659	sizeof(rsp), &rsp);
660	}
661
662	l2cap_chan_del(chan, reason);
663	break;
664
665	case BT_CONNECT:
666	case BT_DISCONN:
667	l2cap_chan_del(chan, reason);
668	break;
669
670	default:
671	chan->ops->teardown(chan, 0);
672	break;
673	}
674	}
675
676	static inline u8 l2cap_get_auth_type(struct l2cap_chan *chan)
677	{
678	switch (chan->chan_type) {
679	case L2CAP_CHAN_RAW:
680	switch (chan->sec_level) {
681	case BT_SECURITY_HIGH:
682	return HCI_AT_DEDICATED_BONDING_MITM;
683	case BT_SECURITY_MEDIUM:
684	return HCI_AT_DEDICATED_BONDING;
685	default:
686	return HCI_AT_NO_BONDING;
687	}
688	break;
689	case L2CAP_CHAN_CONN_LESS:
690	if (chan->psm == __constant_cpu_to_le16(L2CAP_PSM_3DSP)) {
691	if (chan->sec_level == BT_SECURITY_LOW)
692	chan->sec_level = BT_SECURITY_SDP;
693	}
694	if (chan->sec_level == BT_SECURITY_HIGH)
695	return HCI_AT_NO_BONDING_MITM;
696	else
697	return HCI_AT_NO_BONDING;
698	break;
699	case L2CAP_CHAN_CONN_ORIENTED:
700	if (chan->psm == __constant_cpu_to_le16(L2CAP_PSM_SDP)) {
701	if (chan->sec_level == BT_SECURITY_LOW)
702	chan->sec_level = BT_SECURITY_SDP;
703
704	if (chan->sec_level == BT_SECURITY_HIGH)
705	return HCI_AT_NO_BONDING_MITM;
706	else
707	return HCI_AT_NO_BONDING;
708	}
709	/* fall through */
710	default:
711	switch (chan->sec_level) {
712	case BT_SECURITY_HIGH:
713	return HCI_AT_GENERAL_BONDING_MITM;
714	case BT_SECURITY_MEDIUM:
715	return HCI_AT_GENERAL_BONDING;
716	default:
717	return HCI_AT_NO_BONDING;
718	}
719	break;
720	}
721	}
722
723	/* Service level security */
724	int l2cap_chan_check_security(struct l2cap_chan *chan)
725	{
726	struct l2cap_conn *conn = chan->conn;
727	__u8 auth_type;
728
729	auth_type = l2cap_get_auth_type(chan);
730
731	return hci_conn_security(conn->hcon, chan->sec_level, auth_type);
732	}
733
734	static u8 l2cap_get_ident(struct l2cap_conn *conn)
735	{
736	u8 id;
737
738	/* Get next available identificator.
739	* 1 - 128 are used by kernel.
740	* 129 - 199 are reserved.
741	* 200 - 254 are used by utilities like l2ping, etc.
742	*/
743
744	spin_lock(&conn->lock);
745
746	if (++conn->tx_ident > 128)
747	conn->tx_ident = 1;
748
749	id = conn->tx_ident;
750
751	spin_unlock(&conn->lock);
752
753	return id;
754	}
755
756	static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len,
757	void *data)
758	{
759	struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data);
760	u8 flags;
761
762	BT_DBG("code 0x%2.2x", code);
763
764	if (!skb)
765	return;
766
767	if (lmp_no_flush_capable(conn->hcon->hdev))
768	flags = ACL_START_NO_FLUSH;
769	else
770	flags = ACL_START;
771
772	bt_cb(skb)->force_active = BT_POWER_FORCE_ACTIVE_ON;
773	skb->priority = HCI_PRIO_MAX;
774
775	hci_send_acl(conn->hchan, skb, flags);
776	}
777
778	static bool __chan_is_moving(struct l2cap_chan *chan)
779	{
780	return chan->move_state != L2CAP_MOVE_STABLE &&
781	chan->move_state != L2CAP_MOVE_WAIT_PREPARE;
782	}
783
784	static void l2cap_do_send(struct l2cap_chan chan, struct sk_buff skb)
785	{
786	struct hci_conn *hcon = chan->conn->hcon;
787	u16 flags;
788
789	BT_DBG("chan %p, skb %p len %d priority %u", chan, skb, skb->len,
790	skb->priority);
791
792	if (chan->hs_hcon && !__chan_is_moving(chan)) {
793	if (chan->hs_hchan)
794	hci_send_acl(chan->hs_hchan, skb, ACL_COMPLETE);
795	else
796	kfree_skb(skb);
797
798	return;
799	}
800
801	if (!test_bit(FLAG_FLUSHABLE, &chan->flags) &&
802	lmp_no_flush_capable(hcon->hdev))
803	flags = ACL_START_NO_FLUSH;
804	else
805	flags = ACL_START;
806
807	bt_cb(skb)->force_active = test_bit(FLAG_FORCE_ACTIVE, &chan->flags);
808	hci_send_acl(chan->conn->hchan, skb, flags);
809	}
810
811	static void __unpack_enhanced_control(u16 enh, struct l2cap_ctrl *control)
812	{
813	control->reqseq = (enh & L2CAP_CTRL_REQSEQ) >> L2CAP_CTRL_REQSEQ_SHIFT;
814	control->final = (enh & L2CAP_CTRL_FINAL) >> L2CAP_CTRL_FINAL_SHIFT;
815
816	if (enh & L2CAP_CTRL_FRAME_TYPE) {
817	/* S-Frame */
818	control->sframe = 1;
819	control->poll = (enh & L2CAP_CTRL_POLL) >> L2CAP_CTRL_POLL_SHIFT;
820	control->super = (enh & L2CAP_CTRL_SUPERVISE) >> L2CAP_CTRL_SUPER_SHIFT;
821
822	control->sar = 0;
823	control->txseq = 0;
824	} else {
825	/* I-Frame */
826	control->sframe = 0;
827	control->sar = (enh & L2CAP_CTRL_SAR) >> L2CAP_CTRL_SAR_SHIFT;
828	control->txseq = (enh & L2CAP_CTRL_TXSEQ) >> L2CAP_CTRL_TXSEQ_SHIFT;
829
830	control->poll = 0;
831	control->super = 0;
832	}
833	}
834
835	static void __unpack_extended_control(u32 ext, struct l2cap_ctrl *control)
836	{
837	control->reqseq = (ext & L2CAP_EXT_CTRL_REQSEQ) >> L2CAP_EXT_CTRL_REQSEQ_SHIFT;
838	control->final = (ext & L2CAP_EXT_CTRL_FINAL) >> L2CAP_EXT_CTRL_FINAL_SHIFT;
839
840	if (ext & L2CAP_EXT_CTRL_FRAME_TYPE) {
841	/* S-Frame */
842	control->sframe = 1;
843	control->poll = (ext & L2CAP_EXT_CTRL_POLL) >> L2CAP_EXT_CTRL_POLL_SHIFT;
844	control->super = (ext & L2CAP_EXT_CTRL_SUPERVISE) >> L2CAP_EXT_CTRL_SUPER_SHIFT;
845
846	control->sar = 0;
847	control->txseq = 0;
848	} else {
849	/* I-Frame */
850	control->sframe = 0;
851	control->sar = (ext & L2CAP_EXT_CTRL_SAR) >> L2CAP_EXT_CTRL_SAR_SHIFT;
852	control->txseq = (ext & L2CAP_EXT_CTRL_TXSEQ) >> L2CAP_EXT_CTRL_TXSEQ_SHIFT;
853
854	control->poll = 0;
855	control->super = 0;
856	}
857	}
858
859	static inline void __unpack_control(struct l2cap_chan *chan,
860	struct sk_buff *skb)
861	{
862	if (test_bit(FLAG_EXT_CTRL, &chan->flags)) {
863	__unpack_extended_control(get_unaligned_le32(skb->data),
864	&bt_cb(skb)->control);
865	skb_pull(skb, L2CAP_EXT_CTRL_SIZE);
866	} else {
867	__unpack_enhanced_control(get_unaligned_le16(skb->data),
868	&bt_cb(skb)->control);
869	skb_pull(skb, L2CAP_ENH_CTRL_SIZE);
870	}
871	}
872
873	static u32 __pack_extended_control(struct l2cap_ctrl *control)
874	{
875	u32 packed;
876
877	packed = control->reqseq << L2CAP_EXT_CTRL_REQSEQ_SHIFT;
878	packed \|= control->final << L2CAP_EXT_CTRL_FINAL_SHIFT;
879
880	if (control->sframe) {
881	packed \|= control->poll << L2CAP_EXT_CTRL_POLL_SHIFT;
882	packed \|= control->super << L2CAP_EXT_CTRL_SUPER_SHIFT;
883	packed \|= L2CAP_EXT_CTRL_FRAME_TYPE;
884	} else {
885	packed \|= control->sar << L2CAP_EXT_CTRL_SAR_SHIFT;
886	packed \|= control->txseq << L2CAP_EXT_CTRL_TXSEQ_SHIFT;
887	}
888
889	return packed;
890	}
891
892	static u16 __pack_enhanced_control(struct l2cap_ctrl *control)
893	{
894	u16 packed;
895
896	packed = control->reqseq << L2CAP_CTRL_REQSEQ_SHIFT;
897	packed \|= control->final << L2CAP_CTRL_FINAL_SHIFT;
898
899	if (control->sframe) {
900	packed \|= control->poll << L2CAP_CTRL_POLL_SHIFT;
901	packed \|= control->super << L2CAP_CTRL_SUPER_SHIFT;
902	packed \|= L2CAP_CTRL_FRAME_TYPE;
903	} else {
904	packed \|= control->sar << L2CAP_CTRL_SAR_SHIFT;
905	packed \|= control->txseq << L2CAP_CTRL_TXSEQ_SHIFT;
906	}
907
908	return packed;
909	}
910
911	static inline void __pack_control(struct l2cap_chan *chan,
912	struct l2cap_ctrl *control,
913	struct sk_buff *skb)
914	{
915	if (test_bit(FLAG_EXT_CTRL, &chan->flags)) {
916	put_unaligned_le32(__pack_extended_control(control),
917	skb->data + L2CAP_HDR_SIZE);
918	} else {
919	put_unaligned_le16(__pack_enhanced_control(control),
920	skb->data + L2CAP_HDR_SIZE);
921	}
922	}
923
924	static inline unsigned int __ertm_hdr_size(struct l2cap_chan *chan)
925	{
926	if (test_bit(FLAG_EXT_CTRL, &chan->flags))
927	return L2CAP_EXT_HDR_SIZE;
928	else
929	return L2CAP_ENH_HDR_SIZE;
930	}
931
932	static struct sk_buff l2cap_create_sframe_pdu(struct l2cap_chan chan,
933	u32 control)
934	{
935	struct sk_buff *skb;
936	struct l2cap_hdr *lh;
937	int hlen = __ertm_hdr_size(chan);
938
939	if (chan->fcs == L2CAP_FCS_CRC16)
940	hlen += L2CAP_FCS_SIZE;
941
942	skb = bt_skb_alloc(hlen, GFP_KERNEL);
943
944	if (!skb)
945	return ERR_PTR(-ENOMEM);
946
947	lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
948	lh->len = cpu_to_le16(hlen - L2CAP_HDR_SIZE);
949	lh->cid = cpu_to_le16(chan->dcid);
950
951	if (test_bit(FLAG_EXT_CTRL, &chan->flags))
952	put_unaligned_le32(control, skb_put(skb, L2CAP_EXT_CTRL_SIZE));
953	else
954	put_unaligned_le16(control, skb_put(skb, L2CAP_ENH_CTRL_SIZE));
955
956	if (chan->fcs == L2CAP_FCS_CRC16) {
957	u16 fcs = crc16(0, (u8 *)skb->data, skb->len);
958	put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE));
959	}
960
961	skb->priority = HCI_PRIO_MAX;
962	return skb;
963	}
964
965	static void l2cap_send_sframe(struct l2cap_chan *chan,
966	struct l2cap_ctrl *control)
967	{
968	struct sk_buff *skb;
969	u32 control_field;
970
971	BT_DBG("chan %p, control %p", chan, control);
972
973	if (!control->sframe)
974	return;
975
976	if (__chan_is_moving(chan))
977	return;
978
979	if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state) &&
980	!control->poll)
981	control->final = 1;
982
983	if (control->super == L2CAP_SUPER_RR)
984	clear_bit(CONN_RNR_SENT, &chan->conn_state);
985	else if (control->super == L2CAP_SUPER_RNR)
986	set_bit(CONN_RNR_SENT, &chan->conn_state);
987
988	if (control->super != L2CAP_SUPER_SREJ) {
989	chan->last_acked_seq = control->reqseq;
990	__clear_ack_timer(chan);
991	}
992
993	BT_DBG("reqseq %d, final %d, poll %d, super %d", control->reqseq,
994	control->final, control->poll, control->super);
995
996	if (test_bit(FLAG_EXT_CTRL, &chan->flags))
997	control_field = __pack_extended_control(control);
998	else
999	control_field = __pack_enhanced_control(control);
1000
1001	skb = l2cap_create_sframe_pdu(chan, control_field);
1002	if (!IS_ERR(skb))
1003	l2cap_do_send(chan, skb);
1004	}
1005
1006	static void l2cap_send_rr_or_rnr(struct l2cap_chan *chan, bool poll)
1007	{
1008	struct l2cap_ctrl control;
1009
1010	BT_DBG("chan %p, poll %d", chan, poll);
1011
1012	memset(&control, 0, sizeof(control));
1013	control.sframe = 1;
1014	control.poll = poll;
1015
1016	if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state))
1017	control.super = L2CAP_SUPER_RNR;
1018	else
1019	control.super = L2CAP_SUPER_RR;
1020
1021	control.reqseq = chan->buffer_seq;
1022	l2cap_send_sframe(chan, &control);
1023	}
1024
1025	static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan)
1026	{
1027	return !test_bit(CONF_CONNECT_PEND, &chan->conf_state);
1028	}
1029
1030	static bool __amp_capable(struct l2cap_chan *chan)
1031	{
1032	struct l2cap_conn *conn = chan->conn;
1033	struct hci_dev *hdev;
1034	bool amp_available = false;
1035
1036	if (!conn->hs_enabled)
1037	return false;
1038
1039	if (!(conn->fixed_chan_mask & L2CAP_FC_A2MP))
1040	return false;
1041
1042	read_lock(&hci_dev_list_lock);
1043	list_for_each_entry(hdev, &hci_dev_list, list) {
1044	if (hdev->amp_type != AMP_TYPE_BREDR &&
1045	test_bit(HCI_UP, &hdev->flags)) {
1046	amp_available = true;
1047	break;
1048	}
1049	}
1050	read_unlock(&hci_dev_list_lock);
1051
1052	if (chan->chan_policy == BT_CHANNEL_POLICY_AMP_PREFERRED)
1053	return amp_available;
1054
1055	return false;
1056	}
1057
1058	static bool l2cap_check_efs(struct l2cap_chan *chan)
1059	{
1060	/* Check EFS parameters */
1061	return true;
1062	}
1063
1064	void l2cap_send_conn_req(struct l2cap_chan *chan)
1065	{
1066	struct l2cap_conn *conn = chan->conn;
1067	struct l2cap_conn_req req;
1068
1069	req.scid = cpu_to_le16(chan->scid);
1070	req.psm = chan->psm;
1071
1072	chan->ident = l2cap_get_ident(conn);
1073
1074	set_bit(CONF_CONNECT_PEND, &chan->conf_state);
1075
1076	l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_REQ, sizeof(req), &req);
1077	}
1078
1079	static void l2cap_send_create_chan_req(struct l2cap_chan *chan, u8 amp_id)
1080	{
1081	struct l2cap_create_chan_req req;
1082	req.scid = cpu_to_le16(chan->scid);
1083	req.psm = chan->psm;
1084	req.amp_id = amp_id;
1085
1086	chan->ident = l2cap_get_ident(chan->conn);
1087
1088	l2cap_send_cmd(chan->conn, chan->ident, L2CAP_CREATE_CHAN_REQ,
1089	sizeof(req), &req);
1090	}
1091
1092	static void l2cap_move_setup(struct l2cap_chan *chan)
1093	{
1094	struct sk_buff *skb;
1095
1096	BT_DBG("chan %p", chan);
1097
1098	if (chan->mode != L2CAP_MODE_ERTM)
1099	return;
1100
1101	__clear_retrans_timer(chan);
1102	__clear_monitor_timer(chan);
1103	__clear_ack_timer(chan);
1104
1105	chan->retry_count = 0;
1106	skb_queue_walk(&chan->tx_q, skb) {
1107	if (bt_cb(skb)->control.retries)
1108	bt_cb(skb)->control.retries = 1;
1109	else
1110	break;
1111	}
1112
1113	chan->expected_tx_seq = chan->buffer_seq;
1114
1115	clear_bit(CONN_REJ_ACT, &chan->conn_state);
1116	clear_bit(CONN_SREJ_ACT, &chan->conn_state);
1117	l2cap_seq_list_clear(&chan->retrans_list);
1118	l2cap_seq_list_clear(&chan->srej_list);
1119	skb_queue_purge(&chan->srej_q);
1120
1121	chan->tx_state = L2CAP_TX_STATE_XMIT;
1122	chan->rx_state = L2CAP_RX_STATE_MOVE;
1123
1124	set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
1125	}
1126
1127	static void l2cap_move_done(struct l2cap_chan *chan)
1128	{
1129	u8 move_role = chan->move_role;
1130	BT_DBG("chan %p", chan);
1131
1132	chan->move_state = L2CAP_MOVE_STABLE;
1133	chan->move_role = L2CAP_MOVE_ROLE_NONE;
1134
1135	if (chan->mode != L2CAP_MODE_ERTM)
1136	return;
1137
1138	switch (move_role) {
1139	case L2CAP_MOVE_ROLE_INITIATOR:
1140	l2cap_tx(chan, NULL, NULL, L2CAP_EV_EXPLICIT_POLL);
1141	chan->rx_state = L2CAP_RX_STATE_WAIT_F;
1142	break;
1143	case L2CAP_MOVE_ROLE_RESPONDER:
1144	chan->rx_state = L2CAP_RX_STATE_WAIT_P;
1145	break;
1146	}
1147	}
1148
1149	static void l2cap_chan_ready(struct l2cap_chan *chan)
1150	{
1151	/* This clears all conf flags, including CONF_NOT_COMPLETE */
1152	chan->conf_state = 0;
1153	__clear_chan_timer(chan);
1154
1155	chan->state = BT_CONNECTED;
1156
1157	chan->ops->ready(chan);
1158	}
1159
1160	static void l2cap_start_connection(struct l2cap_chan *chan)
1161	{
1162	if (__amp_capable(chan)) {
1163	BT_DBG("chan %p AMP capable: discover AMPs", chan);
1164	a2mp_discover_amp(chan);
1165	} else {
1166	l2cap_send_conn_req(chan);
1167	}
1168	}
1169
1170	static void l2cap_do_start(struct l2cap_chan *chan)
1171	{
1172	struct l2cap_conn *conn = chan->conn;
1173
1174	if (conn->hcon->type == LE_LINK) {
1175	l2cap_chan_ready(chan);
1176	return;
1177	}
1178
1179	if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) {
1180	if (!(conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE))
1181	return;
1182
1183	if (l2cap_chan_check_security(chan) &&
1184	__l2cap_no_conn_pending(chan)) {
1185	l2cap_start_connection(chan);
1186	}
1187	} else {
1188	struct l2cap_info_req req;
1189	req.type = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
1190
1191	conn->info_state \|= L2CAP_INFO_FEAT_MASK_REQ_SENT;
1192	conn->info_ident = l2cap_get_ident(conn);
1193
1194	schedule_delayed_work(&conn->info_timer, L2CAP_INFO_TIMEOUT);
1195
1196	l2cap_send_cmd(conn, conn->info_ident, L2CAP_INFO_REQ,
1197	sizeof(req), &req);
1198	}
1199	}
1200
1201	static inline int l2cap_mode_supported(__u8 mode, __u32 feat_mask)
1202	{
1203	u32 local_feat_mask = l2cap_feat_mask;
1204	if (!disable_ertm)
1205	local_feat_mask \|= L2CAP_FEAT_ERTM \| L2CAP_FEAT_STREAMING;
1206
1207	switch (mode) {
1208	case L2CAP_MODE_ERTM:
1209	return L2CAP_FEAT_ERTM & feat_mask & local_feat_mask;
1210	case L2CAP_MODE_STREAMING:
1211	return L2CAP_FEAT_STREAMING & feat_mask & local_feat_mask;
1212	default:
1213	return 0x00;
1214	}
1215	}
1216
1217	static void l2cap_send_disconn_req(struct l2cap_chan *chan, int err)
1218	{
1219	struct l2cap_conn *conn = chan->conn;
1220	struct l2cap_disconn_req req;
1221
1222	if (!conn)
1223	return;
1224
1225	if (chan->mode == L2CAP_MODE_ERTM && chan->state == BT_CONNECTED) {
1226	__clear_retrans_timer(chan);
1227	__clear_monitor_timer(chan);
1228	__clear_ack_timer(chan);
1229	}
1230
1231	if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP) {
1232	l2cap_state_change(chan, BT_DISCONN);
1233	return;
1234	}
1235
1236	req.dcid = cpu_to_le16(chan->dcid);
1237	req.scid = cpu_to_le16(chan->scid);
1238	l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_DISCONN_REQ,
1239	sizeof(req), &req);
1240
1241	l2cap_state_change_and_error(chan, BT_DISCONN, err);
1242	}
1243
1244	/* ---- L2CAP connections ---- */
1245	static void l2cap_conn_start(struct l2cap_conn *conn)
1246	{
1247	struct l2cap_chan chan, tmp;
1248
1249	BT_DBG("conn %p", conn);
1250
1251	mutex_lock(&conn->chan_lock);
1252
1253	list_for_each_entry_safe(chan, tmp, &conn->chan_l, list) {
1254	l2cap_chan_lock(chan);
1255
1256	if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
1257	l2cap_chan_unlock(chan);
1258	continue;
1259	}
1260
1261	if (chan->state == BT_CONNECT) {
1262	if (!l2cap_chan_check_security(chan) \|\|
1263	!__l2cap_no_conn_pending(chan)) {
1264	l2cap_chan_unlock(chan);
1265	continue;
1266	}
1267
1268	if (!l2cap_mode_supported(chan->mode, conn->feat_mask)
1269	&& test_bit(CONF_STATE2_DEVICE,
1270	&chan->conf_state)) {
1271	l2cap_chan_close(chan, ECONNRESET);
1272	l2cap_chan_unlock(chan);
1273	continue;
1274	}
1275
1276	l2cap_start_connection(chan);
1277
1278	} else if (chan->state == BT_CONNECT2) {
1279	struct l2cap_conn_rsp rsp;
1280	char buf[128];
1281	rsp.scid = cpu_to_le16(chan->dcid);
1282	rsp.dcid = cpu_to_le16(chan->scid);
1283
1284	if (l2cap_chan_check_security(chan)) {
1285	if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
1286	rsp.result = __constant_cpu_to_le16(L2CAP_CR_PEND);
1287	rsp.status = __constant_cpu_to_le16(L2CAP_CS_AUTHOR_PEND);
1288	chan->ops->defer(chan);
1289
1290	} else {
1291	l2cap_state_change(chan, BT_CONFIG);
1292	rsp.result = __constant_cpu_to_le16(L2CAP_CR_SUCCESS);
1293	rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
1294	}
1295	} else {
1296	rsp.result = __constant_cpu_to_le16(L2CAP_CR_PEND);
1297	rsp.status = __constant_cpu_to_le16(L2CAP_CS_AUTHEN_PEND);
1298	}
1299
1300	l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
1301	sizeof(rsp), &rsp);
1302
1303	if (test_bit(CONF_REQ_SENT, &chan->conf_state) \|\|
1304	rsp.result != L2CAP_CR_SUCCESS) {
1305	l2cap_chan_unlock(chan);
1306	continue;
1307	}
1308
1309	set_bit(CONF_REQ_SENT, &chan->conf_state);
1310	l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
1311	l2cap_build_conf_req(chan, buf), buf);
1312	chan->num_conf_req++;
1313	}
1314
1315	l2cap_chan_unlock(chan);
1316	}
1317
1318	mutex_unlock(&conn->chan_lock);
1319	}
1320
1321	/* Find socket with cid and source/destination bdaddr.
1322	* Returns closest match, locked.
1323	*/
1324	static struct l2cap_chan *l2cap_global_chan_by_scid(int state, u16 cid,
1325	bdaddr_t *src,
1326	bdaddr_t *dst)
1327	{
1328	struct l2cap_chan c, c1 = NULL;
1329
1330	read_lock(&chan_list_lock);
1331
1332	list_for_each_entry(c, &chan_list, global_l) {
1333	if (state && c->state != state)
1334	continue;
1335
1336	if (c->scid == cid) {
1337	int src_match, dst_match;
1338	int src_any, dst_any;
1339
1340	/* Exact match. */
1341	src_match = !bacmp(&c->src, src);
1342	dst_match = !bacmp(&c->dst, dst);
1343	if (src_match && dst_match) {
1344	read_unlock(&chan_list_lock);
1345	return c;
1346	}
1347
1348	/* Closest match */
1349	src_any = !bacmp(&c->src, BDADDR_ANY);
1350	dst_any = !bacmp(&c->dst, BDADDR_ANY);
1351	if ((src_match && dst_any) \|\| (src_any && dst_match) \|\|
1352	(src_any && dst_any))
1353	c1 = c;
1354	}
1355	}
1356
1357	read_unlock(&chan_list_lock);
1358
1359	return c1;
1360	}
1361
1362	static void l2cap_le_conn_ready(struct l2cap_conn *conn)
1363	{
1364	struct hci_conn *hcon = conn->hcon;
1365	struct l2cap_chan chan, pchan;
1366	u8 dst_type;
1367
1368	BT_DBG("");
1369
1370	/* Check if we have socket listening on cid */
1371	pchan = l2cap_global_chan_by_scid(BT_LISTEN, L2CAP_CID_ATT,
1372	&hcon->src, &hcon->dst);
1373	if (!pchan)
1374	return;
1375
1376	/* Client ATT sockets should override the server one */
1377	if (__l2cap_get_chan_by_dcid(conn, L2CAP_CID_ATT))
1378	return;
1379
1380	dst_type = bdaddr_type(hcon, hcon->dst_type);
1381
1382	/* If device is blocked, do not create a channel for it */
1383	if (hci_blacklist_lookup(hcon->hdev, &hcon->dst, dst_type))
1384	return;
1385
1386	l2cap_chan_lock(pchan);
1387
1388	chan = pchan->ops->new_connection(pchan);
1389	if (!chan)
1390	goto clean;
1391
1392	chan->dcid = L2CAP_CID_ATT;
1393
1394	bacpy(&chan->src, &hcon->src);
1395	bacpy(&chan->dst, &hcon->dst);
1396	chan->src_type = bdaddr_type(hcon, hcon->src_type);
1397	chan->dst_type = dst_type;
1398
1399	__l2cap_chan_add(conn, chan);
1400
1401	clean:
1402	l2cap_chan_unlock(pchan);
1403	}
1404
1405	static void l2cap_conn_ready(struct l2cap_conn *conn)
1406	{
1407	struct l2cap_chan *chan;
1408	struct hci_conn *hcon = conn->hcon;
1409
1410	BT_DBG("conn %p", conn);
1411
1412	/* For outgoing pairing which doesn't necessarily have an
1413	* associated socket (e.g. mgmt_pair_device).
1414	*/
1415	if (hcon->out && hcon->type == LE_LINK)
1416	smp_conn_security(hcon, hcon->pending_sec_level);
1417
1418	mutex_lock(&conn->chan_lock);
1419
1420	if (hcon->type == LE_LINK)
1421	l2cap_le_conn_ready(conn);
1422
1423	list_for_each_entry(chan, &conn->chan_l, list) {
1424
1425	l2cap_chan_lock(chan);
1426
1427	if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP) {
1428	l2cap_chan_unlock(chan);
1429	continue;
1430	}
1431
1432	if (hcon->type == LE_LINK) {
1433	if (smp_conn_security(hcon, chan->sec_level))
1434	l2cap_chan_ready(chan);
1435
1436	} else if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
1437	l2cap_chan_ready(chan);
1438
1439	} else if (chan->state == BT_CONNECT) {
1440	l2cap_do_start(chan);
1441	}
1442
1443	l2cap_chan_unlock(chan);
1444	}
1445
1446	mutex_unlock(&conn->chan_lock);
1447	}
1448
1449	/* Notify sockets that we cannot guaranty reliability anymore */
1450	static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err)
1451	{
1452	struct l2cap_chan *chan;
1453
1454	BT_DBG("conn %p", conn);
1455
1456	mutex_lock(&conn->chan_lock);
1457
1458	list_for_each_entry(chan, &conn->chan_l, list) {
1459	if (test_bit(FLAG_FORCE_RELIABLE, &chan->flags))
1460	l2cap_chan_set_err(chan, err);
1461	}
1462
1463	mutex_unlock(&conn->chan_lock);
1464	}
1465
1466	static void l2cap_info_timeout(struct work_struct *work)
1467	{
1468	struct l2cap_conn *conn = container_of(work, struct l2cap_conn,
1469	info_timer.work);
1470
1471	conn->info_state \|= L2CAP_INFO_FEAT_MASK_REQ_DONE;
1472	conn->info_ident = 0;
1473
1474	l2cap_conn_start(conn);
1475	}
1476
1477	/*
1478	* l2cap_user
1479	* External modules can register l2cap_user objects on l2cap_conn. The ->probe
1480	* callback is called during registration. The ->remove callback is called
1481	* during unregistration.
1482	* An l2cap_user object can either be explicitly unregistered or when the
1483	* underlying l2cap_conn object is deleted. This guarantees that l2cap->hcon,
1484	* l2cap->hchan, .. are valid as long as the remove callback hasn't been called.
1485	* External modules must own a reference to the l2cap_conn object if they intend
1486	* to call l2cap_unregister_user(). The l2cap_conn object might get destroyed at
1487	* any time if they don't.
1488	*/
1489
1490	int l2cap_register_user(struct l2cap_conn conn, struct l2cap_user user)
1491	{
1492	struct hci_dev *hdev = conn->hcon->hdev;
1493	int ret;
1494
1495	/* We need to check whether l2cap_conn is registered. If it is not, we
1496	* must not register the l2cap_user. l2cap_conn_del() is unregisters
1497	* l2cap_conn objects, but doesn't provide its own locking. Instead, it
1498	* relies on the parent hci_conn object to be locked. This itself relies
1499	* on the hci_dev object to be locked. So we must lock the hci device
1500	* here, too. */
1501
1502	hci_dev_lock(hdev);
1503
1504	if (user->list.next \|\| user->list.prev) {
1505	ret = -EINVAL;
1506	goto out_unlock;
1507	}
1508
1509	/* conn->hchan is NULL after l2cap_conn_del() was called */
1510	if (!conn->hchan) {
1511	ret = -ENODEV;
1512	goto out_unlock;
1513	}
1514
1515	ret = user->probe(conn, user);
1516	if (ret)
1517	goto out_unlock;
1518
1519	list_add(&user->list, &conn->users);
1520	ret = 0;
1521
1522	out_unlock:
1523	hci_dev_unlock(hdev);
1524	return ret;
1525	}
1526	EXPORT_SYMBOL(l2cap_register_user);
1527
1528	void l2cap_unregister_user(struct l2cap_conn conn, struct l2cap_user user)
1529	{
1530	struct hci_dev *hdev = conn->hcon->hdev;
1531
1532	hci_dev_lock(hdev);
1533
1534	if (!user->list.next \|\| !user->list.prev)
1535	goto out_unlock;
1536
1537	list_del(&user->list);
1538	user->list.next = NULL;
1539	user->list.prev = NULL;
1540	user->remove(conn, user);
1541
1542	out_unlock:
1543	hci_dev_unlock(hdev);
1544	}
1545	EXPORT_SYMBOL(l2cap_unregister_user);
1546
1547	static void l2cap_unregister_all_users(struct l2cap_conn *conn)
1548	{
1549	struct l2cap_user *user;
1550
1551	while (!list_empty(&conn->users)) {
1552	user = list_first_entry(&conn->users, struct l2cap_user, list);
1553	list_del(&user->list);
1554	user->list.next = NULL;
1555	user->list.prev = NULL;
1556	user->remove(conn, user);
1557	}
1558	}
1559
1560	static void l2cap_conn_del(struct hci_conn *hcon, int err)
1561	{
1562	struct l2cap_conn *conn = hcon->l2cap_data;
1563	struct l2cap_chan chan, l;
1564
1565	if (!conn)
1566	return;
1567
1568	BT_DBG("hcon %p conn %p, err %d", hcon, conn, err);
1569
1570	kfree_skb(conn->rx_skb);
1571
1572	l2cap_unregister_all_users(conn);
1573
1574	mutex_lock(&conn->chan_lock);
1575
1576	/* Kill channels */
1577	list_for_each_entry_safe(chan, l, &conn->chan_l, list) {
1578	l2cap_chan_hold(chan);
1579	l2cap_chan_lock(chan);
1580
1581	l2cap_chan_del(chan, err);
1582
1583	l2cap_chan_unlock(chan);
1584
1585	chan->ops->close(chan);
1586	l2cap_chan_put(chan);
1587	}
1588
1589	mutex_unlock(&conn->chan_lock);
1590
1591	hci_chan_del(conn->hchan);
1592
1593	if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT)
1594	cancel_delayed_work_sync(&conn->info_timer);
1595
1596	if (test_and_clear_bit(HCI_CONN_LE_SMP_PEND, &hcon->flags)) {
1597	cancel_delayed_work_sync(&conn->security_timer);
1598	smp_chan_destroy(conn);
1599	}
1600
1601	hcon->l2cap_data = NULL;
1602	conn->hchan = NULL;
1603	l2cap_conn_put(conn);
1604	}
1605
1606	static void security_timeout(struct work_struct *work)
1607	{
1608	struct l2cap_conn *conn = container_of(work, struct l2cap_conn,
1609	security_timer.work);
1610
1611	BT_DBG("conn %p", conn);
1612
1613	if (test_and_clear_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->flags)) {
1614	smp_chan_destroy(conn);
1615	l2cap_conn_del(conn->hcon, ETIMEDOUT);
1616	}
1617	}
1618
1619	static struct l2cap_conn l2cap_conn_add(struct hci_conn hcon)
1620	{
1621	struct l2cap_conn *conn = hcon->l2cap_data;
1622	struct hci_chan *hchan;
1623
1624	if (conn)
1625	return conn;
1626
1627	hchan = hci_chan_create(hcon);
1628	if (!hchan)
1629	return NULL;
1630
1631	conn = kzalloc(sizeof(struct l2cap_conn), GFP_KERNEL);
1632	if (!conn) {
1633	hci_chan_del(hchan);
1634	return NULL;
1635	}
1636
1637	kref_init(&conn->ref);
1638	hcon->l2cap_data = conn;
1639	conn->hcon = hcon;
1640	hci_conn_get(conn->hcon);
1641	conn->hchan = hchan;
1642
1643	BT_DBG("hcon %p conn %p hchan %p", hcon, conn, hchan);
1644
1645	switch (hcon->type) {
1646	case LE_LINK:
1647	if (hcon->hdev->le_mtu) {
1648	conn->mtu = hcon->hdev->le_mtu;
1649	break;
1650	}
1651	/* fall through */
1652	default:
1653	conn->mtu = hcon->hdev->acl_mtu;
1654	break;
1655	}
1656
1657	conn->feat_mask = 0;
1658
1659	if (hcon->type == ACL_LINK)
1660	conn->hs_enabled = test_bit(HCI_HS_ENABLED,
1661	&hcon->hdev->dev_flags);
1662
1663	spin_lock_init(&conn->lock);
1664	mutex_init(&conn->chan_lock);
1665
1666	INIT_LIST_HEAD(&conn->chan_l);
1667	INIT_LIST_HEAD(&conn->users);
1668
1669	if (hcon->type == LE_LINK)
1670	INIT_DELAYED_WORK(&conn->security_timer, security_timeout);
1671	else
1672	INIT_DELAYED_WORK(&conn->info_timer, l2cap_info_timeout);
1673
1674	conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM;
1675
1676	return conn;
1677	}
1678
1679	static void l2cap_conn_free(struct kref *ref)
1680	{
1681	struct l2cap_conn *conn = container_of(ref, struct l2cap_conn, ref);
1682
1683	hci_conn_put(conn->hcon);
1684	kfree(conn);
1685	}
1686
1687	void l2cap_conn_get(struct l2cap_conn *conn)
1688	{
1689	kref_get(&conn->ref);
1690	}
1691	EXPORT_SYMBOL(l2cap_conn_get);
1692
1693	void l2cap_conn_put(struct l2cap_conn *conn)
1694	{
1695	kref_put(&conn->ref, l2cap_conn_free);
1696	}
1697	EXPORT_SYMBOL(l2cap_conn_put);
1698
1699	/* ---- Socket interface ---- */
1700
1701	/* Find socket with psm and source / destination bdaddr.
1702	* Returns closest match.
1703	*/
1704	static struct l2cap_chan *l2cap_global_chan_by_psm(int state, __le16 psm,
1705	bdaddr_t *src,
1706	bdaddr_t *dst)
1707	{
1708	struct l2cap_chan c, c1 = NULL;
1709
1710	read_lock(&chan_list_lock);
1711
1712	list_for_each_entry(c, &chan_list, global_l) {
1713	if (state && c->state != state)
1714	continue;
1715
1716	if (c->psm == psm) {
1717	int src_match, dst_match;
1718	int src_any, dst_any;
1719
1720	/* Exact match. */
1721	src_match = !bacmp(&c->src, src);
1722	dst_match = !bacmp(&c->dst, dst);
1723	if (src_match && dst_match) {
1724	read_unlock(&chan_list_lock);
1725	return c;
1726	}
1727
1728	/* Closest match */
1729	src_any = !bacmp(&c->src, BDADDR_ANY);
1730	dst_any = !bacmp(&c->dst, BDADDR_ANY);
1731	if ((src_match && dst_any) \|\| (src_any && dst_match) \|\|
1732	(src_any && dst_any))
1733	c1 = c;
1734	}
1735	}
1736
1737	read_unlock(&chan_list_lock);
1738
1739	return c1;
1740	}
1741
1742	int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid,
1743	bdaddr_t *dst, u8 dst_type)
1744	{
1745	struct l2cap_conn *conn;
1746	struct hci_conn *hcon;
1747	struct hci_dev *hdev;
1748	__u8 auth_type;
1749	int err;
1750
1751	BT_DBG("%pMR -> %pMR (type %u) psm 0x%2.2x", &chan->src, dst,
1752	dst_type, __le16_to_cpu(psm));
1753
1754	hdev = hci_get_route(dst, &chan->src);
1755	if (!hdev)
1756	return -EHOSTUNREACH;
1757
1758	hci_dev_lock(hdev);
1759
1760	l2cap_chan_lock(chan);
1761
1762	/* PSM must be odd and lsb of upper byte must be 0 */
1763	if ((__le16_to_cpu(psm) & 0x0101) != 0x0001 && !cid &&
1764	chan->chan_type != L2CAP_CHAN_RAW) {
1765	err = -EINVAL;
1766	goto done;
1767	}
1768
1769	if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED && !(psm \|\| cid)) {
1770	err = -EINVAL;
1771	goto done;
1772	}
1773
1774	switch (chan->mode) {
1775	case L2CAP_MODE_BASIC:
1776	break;
1777	case L2CAP_MODE_ERTM:
1778	case L2CAP_MODE_STREAMING:
1779	if (!disable_ertm)
1780	break;
1781	/* fall through */
1782	default:
1783	err = -ENOTSUPP;
1784	goto done;
1785	}
1786
1787	switch (chan->state) {
1788	case BT_CONNECT:
1789	case BT_CONNECT2:
1790	case BT_CONFIG:
1791	/* Already connecting */
1792	err = 0;
1793	goto done;
1794
1795	case BT_CONNECTED:
1796	/* Already connected */
1797	err = -EISCONN;
1798	goto done;
1799
1800	case BT_OPEN:
1801	case BT_BOUND:
1802	/* Can connect */
1803	break;
1804
1805	default:
1806	err = -EBADFD;
1807	goto done;
1808	}
1809
1810	/* Set destination address and psm */
1811	bacpy(&chan->dst, dst);
1812	chan->dst_type = dst_type;
1813
1814	chan->psm = psm;
1815	chan->dcid = cid;
1816
1817	auth_type = l2cap_get_auth_type(chan);
1818
1819	if (bdaddr_type_is_le(dst_type))
1820	hcon = hci_connect(hdev, LE_LINK, dst, dst_type,
1821	chan->sec_level, auth_type);
1822	else
1823	hcon = hci_connect(hdev, ACL_LINK, dst, dst_type,
1824	chan->sec_level, auth_type);
1825
1826	if (IS_ERR(hcon)) {
1827	err = PTR_ERR(hcon);
1828	goto done;
1829	}
1830
1831	conn = l2cap_conn_add(hcon);
1832	if (!conn) {
1833	hci_conn_drop(hcon);
1834	err = -ENOMEM;
1835	goto done;
1836	}
1837
1838	if (cid && __l2cap_get_chan_by_dcid(conn, cid)) {
1839	hci_conn_drop(hcon);
1840	err = -EBUSY;
1841	goto done;
1842	}
1843
1844	/* Update source addr of the socket */
1845	bacpy(&chan->src, &hcon->src);
1846	chan->src_type = bdaddr_type(hcon, hcon->src_type);
1847
1848	l2cap_chan_unlock(chan);
1849	l2cap_chan_add(conn, chan);
1850	l2cap_chan_lock(chan);
1851
1852	/* l2cap_chan_add takes its own ref so we can drop this one */
1853	hci_conn_drop(hcon);
1854
1855	l2cap_state_change(chan, BT_CONNECT);
1856	__set_chan_timer(chan, chan->ops->get_sndtimeo(chan));
1857
1858	if (hcon->state == BT_CONNECTED) {
1859	if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
1860	__clear_chan_timer(chan);
1861	if (l2cap_chan_check_security(chan))
1862	l2cap_state_change(chan, BT_CONNECTED);
1863	} else
1864	l2cap_do_start(chan);
1865	}
1866
1867	err = 0;
1868
1869	done:
1870	l2cap_chan_unlock(chan);
1871	hci_dev_unlock(hdev);
1872	hci_dev_put(hdev);
1873	return err;
1874	}
1875
1876	static void l2cap_monitor_timeout(struct work_struct *work)
1877	{
1878	struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
1879	monitor_timer.work);
1880
1881	BT_DBG("chan %p", chan);
1882
1883	l2cap_chan_lock(chan);
1884
1885	if (!chan->conn) {
1886	l2cap_chan_unlock(chan);
1887	l2cap_chan_put(chan);
1888	return;
1889	}
1890
1891	l2cap_tx(chan, NULL, NULL, L2CAP_EV_MONITOR_TO);
1892
1893	l2cap_chan_unlock(chan);
1894	l2cap_chan_put(chan);
1895	}
1896
1897	static void l2cap_retrans_timeout(struct work_struct *work)
1898	{
1899	struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
1900	retrans_timer.work);
1901
1902	BT_DBG("chan %p", chan);
1903
1904	l2cap_chan_lock(chan);
1905
1906	if (!chan->conn) {
1907	l2cap_chan_unlock(chan);
1908	l2cap_chan_put(chan);
1909	return;
1910	}
1911
1912	l2cap_tx(chan, NULL, NULL, L2CAP_EV_RETRANS_TO);
1913	l2cap_chan_unlock(chan);
1914	l2cap_chan_put(chan);
1915	}
1916
1917	static void l2cap_streaming_send(struct l2cap_chan *chan,
1918	struct sk_buff_head *skbs)
1919	{
1920	struct sk_buff *skb;
1921	struct l2cap_ctrl *control;
1922
1923	BT_DBG("chan %p, skbs %p", chan, skbs);
1924
1925	if (__chan_is_moving(chan))
1926	return;
1927
1928	skb_queue_splice_tail_init(skbs, &chan->tx_q);
1929
1930	while (!skb_queue_empty(&chan->tx_q)) {
1931
1932	skb = skb_dequeue(&chan->tx_q);
1933
1934	bt_cb(skb)->control.retries = 1;
1935	control = &bt_cb(skb)->control;
1936
1937	control->reqseq = 0;
1938	control->txseq = chan->next_tx_seq;
1939
1940	__pack_control(chan, control, skb);
1941
1942	if (chan->fcs == L2CAP_FCS_CRC16) {
1943	u16 fcs = crc16(0, (u8 *) skb->data, skb->len);
1944	put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE));
1945	}
1946
1947	l2cap_do_send(chan, skb);
1948
1949	BT_DBG("Sent txseq %u", control->txseq);
1950
1951	chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq);
1952	chan->frames_sent++;
1953	}
1954	}
1955
1956	static int l2cap_ertm_send(struct l2cap_chan *chan)
1957	{
1958	struct sk_buff skb, tx_skb;
1959	struct l2cap_ctrl *control;
1960	int sent = 0;
1961
1962	BT_DBG("chan %p", chan);
1963
1964	if (chan->state != BT_CONNECTED)
1965	return -ENOTCONN;
1966
1967	if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
1968	return 0;
1969
1970	if (__chan_is_moving(chan))
1971	return 0;
1972
1973	while (chan->tx_send_head &&
1974	chan->unacked_frames < chan->remote_tx_win &&
1975	chan->tx_state == L2CAP_TX_STATE_XMIT) {
1976
1977	skb = chan->tx_send_head;
1978
1979	bt_cb(skb)->control.retries = 1;
1980	control = &bt_cb(skb)->control;
1981
1982	if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
1983	control->final = 1;
1984
1985	control->reqseq = chan->buffer_seq;
1986	chan->last_acked_seq = chan->buffer_seq;
1987	control->txseq = chan->next_tx_seq;
1988
1989	__pack_control(chan, control, skb);
1990
1991	if (chan->fcs == L2CAP_FCS_CRC16) {
1992	u16 fcs = crc16(0, (u8 *) skb->data, skb->len);
1993	put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE));
1994	}
1995
1996	/* Clone after data has been modified. Data is assumed to be
1997	read-only (for locking purposes) on cloned sk_buffs.
1998	*/
1999	tx_skb = skb_clone(skb, GFP_KERNEL);
2000
2001	if (!tx_skb)
2002	break;
2003
2004	__set_retrans_timer(chan);
2005
2006	chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq);
2007	chan->unacked_frames++;
2008	chan->frames_sent++;
2009	sent++;
2010
2011	if (skb_queue_is_last(&chan->tx_q, skb))
2012	chan->tx_send_head = NULL;
2013	else
2014	chan->tx_send_head = skb_queue_next(&chan->tx_q, skb);
2015
2016	l2cap_do_send(chan, tx_skb);
2017	BT_DBG("Sent txseq %u", control->txseq);
2018	}
2019
2020	BT_DBG("Sent %d, %u unacked, %u in ERTM queue", sent,
2021	chan->unacked_frames, skb_queue_len(&chan->tx_q));
2022
2023	return sent;
2024	}
2025
2026	static void l2cap_ertm_resend(struct l2cap_chan *chan)
2027	{
2028	struct l2cap_ctrl control;
2029	struct sk_buff *skb;
2030	struct sk_buff *tx_skb;
2031	u16 seq;
2032
2033	BT_DBG("chan %p", chan);
2034
2035	if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
2036	return;
2037
2038	if (__chan_is_moving(chan))
2039	return;
2040
2041	while (chan->retrans_list.head != L2CAP_SEQ_LIST_CLEAR) {
2042	seq = l2cap_seq_list_pop(&chan->retrans_list);
2043
2044	skb = l2cap_ertm_seq_in_queue(&chan->tx_q, seq);
2045	if (!skb) {
2046	BT_DBG("Error: Can't retransmit seq %d, frame missing",
2047	seq);
2048	continue;
2049	}
2050
2051	bt_cb(skb)->control.retries++;
2052	control = bt_cb(skb)->control;
2053
2054	if (chan->max_tx != 0 &&
2055	bt_cb(skb)->control.retries > chan->max_tx) {
2056	BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
2057	l2cap_send_disconn_req(chan, ECONNRESET);
2058	l2cap_seq_list_clear(&chan->retrans_list);
2059	break;
2060	}
2061
2062	control.reqseq = chan->buffer_seq;
2063	if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
2064	control.final = 1;
2065	else
2066	control.final = 0;
2067
2068	if (skb_cloned(skb)) {
2069	/* Cloned sk_buffs are read-only, so we need a
2070	* writeable copy
2071	*/
2072	tx_skb = skb_copy(skb, GFP_KERNEL);
2073	} else {
2074	tx_skb = skb_clone(skb, GFP_KERNEL);
2075	}
2076
2077	if (!tx_skb) {
2078	l2cap_seq_list_clear(&chan->retrans_list);
2079	break;
2080	}
2081
2082	/* Update skb contents */
2083	if (test_bit(FLAG_EXT_CTRL, &chan->flags)) {
2084	put_unaligned_le32(__pack_extended_control(&control),
2085	tx_skb->data + L2CAP_HDR_SIZE);
2086	} else {
2087	put_unaligned_le16(__pack_enhanced_control(&control),
2088	tx_skb->data + L2CAP_HDR_SIZE);
2089	}
2090
2091	if (chan->fcs == L2CAP_FCS_CRC16) {
2092	u16 fcs = crc16(0, (u8 *) tx_skb->data, tx_skb->len);
2093	put_unaligned_le16(fcs, skb_put(tx_skb,
2094	L2CAP_FCS_SIZE));
2095	}
2096
2097	l2cap_do_send(chan, tx_skb);
2098
2099	BT_DBG("Resent txseq %d", control.txseq);
2100
2101	chan->last_acked_seq = chan->buffer_seq;
2102	}
2103	}
2104
2105	static void l2cap_retransmit(struct l2cap_chan *chan,
2106	struct l2cap_ctrl *control)
2107	{
2108	BT_DBG("chan %p, control %p", chan, control);
2109
2110	l2cap_seq_list_append(&chan->retrans_list, control->reqseq);
2111	l2cap_ertm_resend(chan);
2112	}
2113
2114	static void l2cap_retransmit_all(struct l2cap_chan *chan,
2115	struct l2cap_ctrl *control)
2116	{
2117	struct sk_buff *skb;
2118
2119	BT_DBG("chan %p, control %p", chan, control);
2120
2121	if (control->poll)
2122	set_bit(CONN_SEND_FBIT, &chan->conn_state);
2123
2124	l2cap_seq_list_clear(&chan->retrans_list);
2125
2126	if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
2127	return;
2128
2129	if (chan->unacked_frames) {
2130	skb_queue_walk(&chan->tx_q, skb) {
2131	if (bt_cb(skb)->control.txseq == control->reqseq \|\|
2132	skb == chan->tx_send_head)
2133	break;
2134	}
2135
2136	skb_queue_walk_from(&chan->tx_q, skb) {
2137	if (skb == chan->tx_send_head)
2138	break;
2139
2140	l2cap_seq_list_append(&chan->retrans_list,
2141	bt_cb(skb)->control.txseq);
2142	}
2143
2144	l2cap_ertm_resend(chan);
2145	}
2146	}
2147
2148	static void l2cap_send_ack(struct l2cap_chan *chan)
2149	{
2150	struct l2cap_ctrl control;
2151	u16 frames_to_ack = __seq_offset(chan, chan->buffer_seq,
2152	chan->last_acked_seq);
2153	int threshold;
2154
2155	BT_DBG("chan %p last_acked_seq %d buffer_seq %d",
2156	chan, chan->last_acked_seq, chan->buffer_seq);
2157
2158	memset(&control, 0, sizeof(control));
2159	control.sframe = 1;
2160
2161	if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state) &&
2162	chan->rx_state == L2CAP_RX_STATE_RECV) {
2163	__clear_ack_timer(chan);
2164	control.super = L2CAP_SUPER_RNR;
2165	control.reqseq = chan->buffer_seq;
2166	l2cap_send_sframe(chan, &control);
2167	} else {
2168	if (!test_bit(CONN_REMOTE_BUSY, &chan->conn_state)) {
2169	l2cap_ertm_send(chan);
2170	/* If any i-frames were sent, they included an ack */
2171	if (chan->buffer_seq == chan->last_acked_seq)
2172	frames_to_ack = 0;
2173	}
2174
2175	/* Ack now if the window is 3/4ths full.
2176	* Calculate without mul or div
2177	*/
2178	threshold = chan->ack_win;
2179	threshold += threshold << 1;
2180	threshold >>= 2;
2181
2182	BT_DBG("frames_to_ack %u, threshold %d", frames_to_ack,
2183	threshold);
2184
2185	if (frames_to_ack >= threshold) {
2186	__clear_ack_timer(chan);
2187	control.super = L2CAP_SUPER_RR;
2188	control.reqseq = chan->buffer_seq;
2189	l2cap_send_sframe(chan, &control);
2190	frames_to_ack = 0;
2191	}
2192
2193	if (frames_to_ack)
2194	__set_ack_timer(chan);
2195	}
2196	}
2197
2198	static inline int l2cap_skbuff_fromiovec(struct l2cap_chan *chan,
2199	struct msghdr *msg, int len,
2200	int count, struct sk_buff *skb)
2201	{
2202	struct l2cap_conn *conn = chan->conn;
2203	struct sk_buff **frag;
2204	int sent = 0;
2205
2206	if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count))
2207	return -EFAULT;
2208
2209	sent += count;
2210	len -= count;
2211
2212	/* Continuation fragments (no L2CAP header) */
2213	frag = &skb_shinfo(skb)->frag_list;
2214	while (len) {
2215	struct sk_buff *tmp;
2216
2217	count = min_t(unsigned int, conn->mtu, len);
2218
2219	tmp = chan->ops->alloc_skb(chan, count,
2220	msg->msg_flags & MSG_DONTWAIT);
2221	if (IS_ERR(tmp))
2222	return PTR_ERR(tmp);
2223
2224	*frag = tmp;
2225
2226	if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count))
2227	return -EFAULT;
2228
2229	(*frag)->priority = skb->priority;
2230
2231	sent += count;
2232	len -= count;
2233
2234	skb->len += (*frag)->len;
2235	skb->data_len += (*frag)->len;
2236
2237	frag = &(*frag)->next;
2238	}
2239
2240	return sent;
2241	}
2242
2243	static struct sk_buff l2cap_create_connless_pdu(struct l2cap_chan chan,
2244	struct msghdr *msg, size_t len,
2245	u32 priority)
2246	{
2247	struct l2cap_conn *conn = chan->conn;
2248	struct sk_buff *skb;
2249	int err, count, hlen = L2CAP_HDR_SIZE + L2CAP_PSMLEN_SIZE;
2250	struct l2cap_hdr *lh;
2251
2252	BT_DBG("chan %p psm 0x%2.2x len %zu priority %u", chan,
2253	__le16_to_cpu(chan->psm), len, priority);
2254
2255	count = min_t(unsigned int, (conn->mtu - hlen), len);
2256
2257	skb = chan->ops->alloc_skb(chan, count + hlen,
2258	msg->msg_flags & MSG_DONTWAIT);
2259	if (IS_ERR(skb))
2260	return skb;
2261
2262	skb->priority = priority;
2263
2264	/* Create L2CAP header */
2265	lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
2266	lh->cid = cpu_to_le16(chan->dcid);
2267	lh->len = cpu_to_le16(len + L2CAP_PSMLEN_SIZE);
2268	put_unaligned(chan->psm, (__le16 *) skb_put(skb, L2CAP_PSMLEN_SIZE));
2269
2270	err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb);
2271	if (unlikely(err < 0)) {
2272	kfree_skb(skb);
2273	return ERR_PTR(err);
2274	}
2275	return skb;
2276	}
2277
2278	static struct sk_buff l2cap_create_basic_pdu(struct l2cap_chan chan,
2279	struct msghdr *msg, size_t len,
2280	u32 priority)
2281	{
2282	struct l2cap_conn *conn = chan->conn;
2283	struct sk_buff *skb;
2284	int err, count;
2285	struct l2cap_hdr *lh;
2286
2287	BT_DBG("chan %p len %zu", chan, len);
2288
2289	count = min_t(unsigned int, (conn->mtu - L2CAP_HDR_SIZE), len);
2290
2291	skb = chan->ops->alloc_skb(chan, count + L2CAP_HDR_SIZE,
2292	msg->msg_flags & MSG_DONTWAIT);
2293	if (IS_ERR(skb))
2294	return skb;
2295
2296	skb->priority = priority;
2297
2298	/* Create L2CAP header */
2299	lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
2300	lh->cid = cpu_to_le16(chan->dcid);
2301	lh->len = cpu_to_le16(len);
2302
2303	err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb);
2304	if (unlikely(err < 0)) {
2305	kfree_skb(skb);
2306	return ERR_PTR(err);
2307	}
2308	return skb;
2309	}
2310
2311	static struct sk_buff l2cap_create_iframe_pdu(struct l2cap_chan chan,
2312	struct msghdr *msg, size_t len,
2313	u16 sdulen)
2314	{
2315	struct l2cap_conn *conn = chan->conn;
2316	struct sk_buff *skb;
2317	int err, count, hlen;
2318	struct l2cap_hdr *lh;
2319
2320	BT_DBG("chan %p len %zu", chan, len);
2321
2322	if (!conn)
2323	return ERR_PTR(-ENOTCONN);
2324
2325	hlen = __ertm_hdr_size(chan);
2326
2327	if (sdulen)
2328	hlen += L2CAP_SDULEN_SIZE;
2329
2330	if (chan->fcs == L2CAP_FCS_CRC16)
2331	hlen += L2CAP_FCS_SIZE;
2332
2333	count = min_t(unsigned int, (conn->mtu - hlen), len);
2334
2335	skb = chan->ops->alloc_skb(chan, count + hlen,
2336	msg->msg_flags & MSG_DONTWAIT);
2337	if (IS_ERR(skb))
2338	return skb;
2339
2340	/* Create L2CAP header */
2341	lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
2342	lh->cid = cpu_to_le16(chan->dcid);
2343	lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
2344
2345	/* Control header is populated later */
2346	if (test_bit(FLAG_EXT_CTRL, &chan->flags))
2347	put_unaligned_le32(0, skb_put(skb, L2CAP_EXT_CTRL_SIZE));
2348	else
2349	put_unaligned_le16(0, skb_put(skb, L2CAP_ENH_CTRL_SIZE));
2350
2351	if (sdulen)
2352	put_unaligned_le16(sdulen, skb_put(skb, L2CAP_SDULEN_SIZE));
2353
2354	err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb);
2355	if (unlikely(err < 0)) {
2356	kfree_skb(skb);
2357	return ERR_PTR(err);
2358	}
2359
2360	bt_cb(skb)->control.fcs = chan->fcs;
2361	bt_cb(skb)->control.retries = 0;
2362	return skb;
2363	}
2364
2365	static int l2cap_segment_sdu(struct l2cap_chan *chan,
2366	struct sk_buff_head *seg_queue,
2367	struct msghdr *msg, size_t len)
2368	{
2369	struct sk_buff *skb;
2370	u16 sdu_len;
2371	size_t pdu_len;
2372	u8 sar;
2373
2374	BT_DBG("chan %p, msg %p, len %zu", chan, msg, len);
2375
2376	/* It is critical that ERTM PDUs fit in a single HCI fragment,
2377	* so fragmented skbs are not used. The HCI layer's handling
2378	* of fragmented skbs is not compatible with ERTM's queueing.
2379	*/
2380
2381	/* PDU size is derived from the HCI MTU */
2382	pdu_len = chan->conn->mtu;
2383
2384	/* Constrain PDU size for BR/EDR connections */
2385	if (!chan->hs_hcon)
2386	pdu_len = min_t(size_t, pdu_len, L2CAP_BREDR_MAX_PAYLOAD);
2387
2388	/* Adjust for largest possible L2CAP overhead. */
2389	if (chan->fcs)
2390	pdu_len -= L2CAP_FCS_SIZE;
2391
2392	pdu_len -= __ertm_hdr_size(chan);
2393
2394	/* Remote device may have requested smaller PDUs */
2395	pdu_len = min_t(size_t, pdu_len, chan->remote_mps);
2396
2397	if (len <= pdu_len) {
2398	sar = L2CAP_SAR_UNSEGMENTED;
2399	sdu_len = 0;
2400	pdu_len = len;
2401	} else {
2402	sar = L2CAP_SAR_START;
2403	sdu_len = len;
2404	pdu_len -= L2CAP_SDULEN_SIZE;
2405	}
2406
2407	while (len > 0) {
2408	skb = l2cap_create_iframe_pdu(chan, msg, pdu_len, sdu_len);
2409
2410	if (IS_ERR(skb)) {
2411	__skb_queue_purge(seg_queue);
2412	return PTR_ERR(skb);
2413	}
2414
2415	bt_cb(skb)->control.sar = sar;
2416	__skb_queue_tail(seg_queue, skb);
2417
2418	len -= pdu_len;
2419	if (sdu_len) {
2420	sdu_len = 0;
2421	pdu_len += L2CAP_SDULEN_SIZE;
2422	}
2423
2424	if (len <= pdu_len) {
2425	sar = L2CAP_SAR_END;
2426	pdu_len = len;
2427	} else {
2428	sar = L2CAP_SAR_CONTINUE;
2429	}
2430	}
2431
2432	return 0;
2433	}
2434
2435	int l2cap_chan_send(struct l2cap_chan chan, struct msghdr msg, size_t len,
2436	u32 priority)
2437	{
2438	struct sk_buff *skb;
2439	int err;
2440	struct sk_buff_head seg_queue;
2441
2442	if (!chan->conn)
2443	return -ENOTCONN;
2444
2445	/* Connectionless channel */
2446	if (chan->chan_type == L2CAP_CHAN_CONN_LESS) {
2447	skb = l2cap_create_connless_pdu(chan, msg, len, priority);
2448	if (IS_ERR(skb))
2449	return PTR_ERR(skb);
2450
2451	l2cap_do_send(chan, skb);
2452	return len;
2453	}
2454
2455	switch (chan->mode) {
2456	case L2CAP_MODE_BASIC:
2457	/* Check outgoing MTU */
2458	if (len > chan->omtu)
2459	return -EMSGSIZE;
2460
2461	/* Create a basic PDU */
2462	skb = l2cap_create_basic_pdu(chan, msg, len, priority);
2463	if (IS_ERR(skb))
2464	return PTR_ERR(skb);
2465
2466	l2cap_do_send(chan, skb);
2467	err = len;
2468	break;
2469
2470	case L2CAP_MODE_ERTM:
2471	case L2CAP_MODE_STREAMING:
2472	/* Check outgoing MTU */
2473	if (len > chan->omtu) {
2474	err = -EMSGSIZE;
2475	break;
2476	}
2477
2478	__skb_queue_head_init(&seg_queue);
2479
2480	/* Do segmentation before calling in to the state machine,
2481	* since it's possible to block while waiting for memory
2482	* allocation.
2483	*/
2484	err = l2cap_segment_sdu(chan, &seg_queue, msg, len);
2485
2486	/* The channel could have been closed while segmenting,
2487	* check that it is still connected.
2488	*/
2489	if (chan->state != BT_CONNECTED) {
2490	__skb_queue_purge(&seg_queue);
2491	err = -ENOTCONN;
2492	}
2493
2494	if (err)
2495	break;
2496
2497	if (chan->mode == L2CAP_MODE_ERTM)
2498	l2cap_tx(chan, NULL, &seg_queue, L2CAP_EV_DATA_REQUEST);
2499	else
2500	l2cap_streaming_send(chan, &seg_queue);
2501
2502	err = len;
2503
2504	/* If the skbs were not queued for sending, they'll still be in
2505	* seg_queue and need to be purged.
2506	*/
2507	__skb_queue_purge(&seg_queue);
2508	break;
2509
2510	default:
2511	BT_DBG("bad state %1.1x", chan->mode);
2512	err = -EBADFD;
2513	}
2514
2515	return err;
2516	}
2517
2518	static void l2cap_send_srej(struct l2cap_chan *chan, u16 txseq)
2519	{
2520	struct l2cap_ctrl control;
2521	u16 seq;
2522
2523	BT_DBG("chan %p, txseq %u", chan, txseq);
2524
2525	memset(&control, 0, sizeof(control));
2526	control.sframe = 1;
2527	control.super = L2CAP_SUPER_SREJ;
2528
2529	for (seq = chan->expected_tx_seq; seq != txseq;
2530	seq = __next_seq(chan, seq)) {
2531	if (!l2cap_ertm_seq_in_queue(&chan->srej_q, seq)) {
2532	control.reqseq = seq;
2533	l2cap_send_sframe(chan, &control);
2534	l2cap_seq_list_append(&chan->srej_list, seq);
2535	}
2536	}
2537
2538	chan->expected_tx_seq = __next_seq(chan, txseq);
2539	}
2540
2541	static void l2cap_send_srej_tail(struct l2cap_chan *chan)
2542	{
2543	struct l2cap_ctrl control;
2544
2545	BT_DBG("chan %p", chan);
2546
2547	if (chan->srej_list.tail == L2CAP_SEQ_LIST_CLEAR)
2548	return;
2549
2550	memset(&control, 0, sizeof(control));
2551	control.sframe = 1;
2552	control.super = L2CAP_SUPER_SREJ;
2553	control.reqseq = chan->srej_list.tail;
2554	l2cap_send_sframe(chan, &control);
2555	}
2556
2557	static void l2cap_send_srej_list(struct l2cap_chan *chan, u16 txseq)
2558	{
2559	struct l2cap_ctrl control;
2560	u16 initial_head;
2561	u16 seq;
2562
2563	BT_DBG("chan %p, txseq %u", chan, txseq);
2564
2565	memset(&control, 0, sizeof(control));
2566	control.sframe = 1;
2567	control.super = L2CAP_SUPER_SREJ;
2568
2569	/* Capture initial list head to allow only one pass through the list. */
2570	initial_head = chan->srej_list.head;
2571
2572	do {
2573	seq = l2cap_seq_list_pop(&chan->srej_list);
2574	if (seq == txseq \|\| seq == L2CAP_SEQ_LIST_CLEAR)
2575	break;
2576
2577	control.reqseq = seq;
2578	l2cap_send_sframe(chan, &control);
2579	l2cap_seq_list_append(&chan->srej_list, seq);
2580	} while (chan->srej_list.head != initial_head);
2581	}
2582
2583	static void l2cap_process_reqseq(struct l2cap_chan *chan, u16 reqseq)
2584	{
2585	struct sk_buff *acked_skb;
2586	u16 ackseq;
2587
2588	BT_DBG("chan %p, reqseq %u", chan, reqseq);
2589
2590	if (chan->unacked_frames == 0 \|\| reqseq == chan->expected_ack_seq)
2591	return;
2592
2593	BT_DBG("expected_ack_seq %u, unacked_frames %u",
2594	chan->expected_ack_seq, chan->unacked_frames);
2595
2596	for (ackseq = chan->expected_ack_seq; ackseq != reqseq;
2597	ackseq = __next_seq(chan, ackseq)) {
2598
2599	acked_skb = l2cap_ertm_seq_in_queue(&chan->tx_q, ackseq);
2600	if (acked_skb) {
2601	skb_unlink(acked_skb, &chan->tx_q);
2602	kfree_skb(acked_skb);
2603	chan->unacked_frames--;
2604	}
2605	}
2606
2607	chan->expected_ack_seq = reqseq;
2608
2609	if (chan->unacked_frames == 0)
2610	__clear_retrans_timer(chan);
2611
2612	BT_DBG("unacked_frames %u", chan->unacked_frames);
2613	}
2614
2615	static void l2cap_abort_rx_srej_sent(struct l2cap_chan *chan)
2616	{
2617	BT_DBG("chan %p", chan);
2618
2619	chan->expected_tx_seq = chan->buffer_seq;
2620	l2cap_seq_list_clear(&chan->srej_list);
2621	skb_queue_purge(&chan->srej_q);
2622	chan->rx_state = L2CAP_RX_STATE_RECV;
2623	}
2624
2625	static void l2cap_tx_state_xmit(struct l2cap_chan *chan,
2626	struct l2cap_ctrl *control,
2627	struct sk_buff_head *skbs, u8 event)
2628	{
2629	BT_DBG("chan %p, control %p, skbs %p, event %d", chan, control, skbs,
2630	event);
2631
2632	switch (event) {
2633	case L2CAP_EV_DATA_REQUEST:
2634	if (chan->tx_send_head == NULL)
2635	chan->tx_send_head = skb_peek(skbs);
2636
2637	skb_queue_splice_tail_init(skbs, &chan->tx_q);
2638	l2cap_ertm_send(chan);
2639	break;
2640	case L2CAP_EV_LOCAL_BUSY_DETECTED:
2641	BT_DBG("Enter LOCAL_BUSY");
2642	set_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2643
2644	if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
2645	/* The SREJ_SENT state must be aborted if we are to
2646	* enter the LOCAL_BUSY state.
2647	*/
2648	l2cap_abort_rx_srej_sent(chan);
2649	}
2650
2651	l2cap_send_ack(chan);
2652
2653	break;
2654	case L2CAP_EV_LOCAL_BUSY_CLEAR:
2655	BT_DBG("Exit LOCAL_BUSY");
2656	clear_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2657
2658	if (test_bit(CONN_RNR_SENT, &chan->conn_state)) {
2659	struct l2cap_ctrl local_control;
2660
2661	memset(&local_control, 0, sizeof(local_control));
2662	local_control.sframe = 1;
2663	local_control.super = L2CAP_SUPER_RR;
2664	local_control.poll = 1;
2665	local_control.reqseq = chan->buffer_seq;
2666	l2cap_send_sframe(chan, &local_control);
2667
2668	chan->retry_count = 1;
2669	__set_monitor_timer(chan);
2670	chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2671	}
2672	break;
2673	case L2CAP_EV_RECV_REQSEQ_AND_FBIT:
2674	l2cap_process_reqseq(chan, control->reqseq);
2675	break;
2676	case L2CAP_EV_EXPLICIT_POLL:
2677	l2cap_send_rr_or_rnr(chan, 1);
2678	chan->retry_count = 1;
2679	__set_monitor_timer(chan);
2680	__clear_ack_timer(chan);
2681	chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2682	break;
2683	case L2CAP_EV_RETRANS_TO:
2684	l2cap_send_rr_or_rnr(chan, 1);
2685	chan->retry_count = 1;
2686	__set_monitor_timer(chan);
2687	chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2688	break;
2689	case L2CAP_EV_RECV_FBIT:
2690	/* Nothing to process */
2691	break;
2692	default:
2693	break;
2694	}
2695	}
2696
2697	static void l2cap_tx_state_wait_f(struct l2cap_chan *chan,
2698	struct l2cap_ctrl *control,
2699	struct sk_buff_head *skbs, u8 event)
2700	{
2701	BT_DBG("chan %p, control %p, skbs %p, event %d", chan, control, skbs,
2702	event);
2703
2704	switch (event) {
2705	case L2CAP_EV_DATA_REQUEST:
2706	if (chan->tx_send_head == NULL)
2707	chan->tx_send_head = skb_peek(skbs);
2708	/* Queue data, but don't send. */
2709	skb_queue_splice_tail_init(skbs, &chan->tx_q);
2710	break;
2711	case L2CAP_EV_LOCAL_BUSY_DETECTED:
2712	BT_DBG("Enter LOCAL_BUSY");
2713	set_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2714
2715	if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
2716	/* The SREJ_SENT state must be aborted if we are to
2717	* enter the LOCAL_BUSY state.
2718	*/
2719	l2cap_abort_rx_srej_sent(chan);
2720	}
2721
2722	l2cap_send_ack(chan);
2723
2724	break;
2725	case L2CAP_EV_LOCAL_BUSY_CLEAR:
2726	BT_DBG("Exit LOCAL_BUSY");
2727	clear_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2728
2729	if (test_bit(CONN_RNR_SENT, &chan->conn_state)) {
2730	struct l2cap_ctrl local_control;
2731	memset(&local_control, 0, sizeof(local_control));
2732	local_control.sframe = 1;
2733	local_control.super = L2CAP_SUPER_RR;
2734	local_control.poll = 1;
2735	local_control.reqseq = chan->buffer_seq;
2736	l2cap_send_sframe(chan, &local_control);
2737
2738	chan->retry_count = 1;
2739	__set_monitor_timer(chan);
2740	chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2741	}
2742	break;
2743	case L2CAP_EV_RECV_REQSEQ_AND_FBIT:
2744	l2cap_process_reqseq(chan, control->reqseq);
2745
2746	/* Fall through */
2747
2748	case L2CAP_EV_RECV_FBIT:
2749	if (control && control->final) {
2750	__clear_monitor_timer(chan);
2751	if (chan->unacked_frames > 0)
2752	__set_retrans_timer(chan);
2753	chan->retry_count = 0;
2754	chan->tx_state = L2CAP_TX_STATE_XMIT;
2755	BT_DBG("recv fbit tx_state 0x2.2%x", chan->tx_state);
2756	}
2757	break;
2758	case L2CAP_EV_EXPLICIT_POLL:
2759	/* Ignore */
2760	break;
2761	case L2CAP_EV_MONITOR_TO:
2762	if (chan->max_tx == 0 \|\| chan->retry_count < chan->max_tx) {
2763	l2cap_send_rr_or_rnr(chan, 1);
2764	__set_monitor_timer(chan);
2765	chan->retry_count++;
2766	} else {
2767	l2cap_send_disconn_req(chan, ECONNABORTED);
2768	}
2769	break;
2770	default:
2771	break;
2772	}
2773	}
2774
2775	static void l2cap_tx(struct l2cap_chan chan, struct l2cap_ctrl control,
2776	struct sk_buff_head *skbs, u8 event)
2777	{
2778	BT_DBG("chan %p, control %p, skbs %p, event %d, state %d",
2779	chan, control, skbs, event, chan->tx_state);
2780
2781	switch (chan->tx_state) {
2782	case L2CAP_TX_STATE_XMIT:
2783	l2cap_tx_state_xmit(chan, control, skbs, event);
2784	break;
2785	case L2CAP_TX_STATE_WAIT_F:
2786	l2cap_tx_state_wait_f(chan, control, skbs, event);
2787	break;
2788	default:
2789	/* Ignore event */
2790	break;
2791	}
2792	}
2793
2794	static void l2cap_pass_to_tx(struct l2cap_chan *chan,
2795	struct l2cap_ctrl *control)
2796	{
2797	BT_DBG("chan %p, control %p", chan, control);
2798	l2cap_tx(chan, control, NULL, L2CAP_EV_RECV_REQSEQ_AND_FBIT);
2799	}
2800
2801	static void l2cap_pass_to_tx_fbit(struct l2cap_chan *chan,
2802	struct l2cap_ctrl *control)
2803	{
2804	BT_DBG("chan %p, control %p", chan, control);
2805	l2cap_tx(chan, control, NULL, L2CAP_EV_RECV_FBIT);
2806	}
2807
2808	/* Copy frame to all raw sockets on that connection */
2809	static void l2cap_raw_recv(struct l2cap_conn conn, struct sk_buff skb)
2810	{
2811	struct sk_buff *nskb;
2812	struct l2cap_chan *chan;
2813
2814	BT_DBG("conn %p", conn);
2815
2816	mutex_lock(&conn->chan_lock);
2817
2818	list_for_each_entry(chan, &conn->chan_l, list) {
2819	if (chan->chan_type != L2CAP_CHAN_RAW)
2820	continue;
2821
2822	/* Don't send frame to the channel it came from */
2823	if (bt_cb(skb)->chan == chan)
2824	continue;
2825
2826	nskb = skb_clone(skb, GFP_KERNEL);
2827	if (!nskb)
2828	continue;
2829	if (chan->ops->recv(chan, nskb))
2830	kfree_skb(nskb);
2831	}
2832
2833	mutex_unlock(&conn->chan_lock);
2834	}
2835
2836	/* ---- L2CAP signalling commands ---- */
2837	static struct sk_buff l2cap_build_cmd(struct l2cap_conn conn, u8 code,
2838	u8 ident, u16 dlen, void *data)
2839	{
2840	struct sk_buff skb, *frag;
2841	struct l2cap_cmd_hdr *cmd;
2842	struct l2cap_hdr *lh;
2843	int len, count;
2844
2845	BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %u",
2846	conn, code, ident, dlen);
2847
2848	if (conn->mtu < L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE)
2849	return NULL;
2850
2851	len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen;
2852	count = min_t(unsigned int, conn->mtu, len);
2853
2854	skb = bt_skb_alloc(count, GFP_KERNEL);
2855	if (!skb)
2856	return NULL;
2857
2858	lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
2859	lh->len = cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen);
2860
2861	if (conn->hcon->type == LE_LINK)
2862	lh->cid = __constant_cpu_to_le16(L2CAP_CID_LE_SIGNALING);
2863	else
2864	lh->cid = __constant_cpu_to_le16(L2CAP_CID_SIGNALING);
2865
2866	cmd = (struct l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE);
2867	cmd->code = code;
2868	cmd->ident = ident;
2869	cmd->len = cpu_to_le16(dlen);
2870
2871	if (dlen) {
2872	count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE;
2873	memcpy(skb_put(skb, count), data, count);
2874	data += count;
2875	}
2876
2877	len -= skb->len;
2878
2879	/* Continuation fragments (no L2CAP header) */
2880	frag = &skb_shinfo(skb)->frag_list;
2881	while (len) {
2882	count = min_t(unsigned int, conn->mtu, len);
2883
2884	*frag = bt_skb_alloc(count, GFP_KERNEL);
2885	if (!*frag)
2886	goto fail;
2887
2888	memcpy(skb_put(*frag, count), data, count);
2889
2890	len -= count;
2891	data += count;
2892
2893	frag = &(*frag)->next;
2894	}
2895
2896	return skb;
2897
2898	fail:
2899	kfree_skb(skb);
2900	return NULL;
2901	}
2902
2903	static inline int l2cap_get_conf_opt(void *ptr, int type, int *olen,
2904	unsigned long *val)
2905	{
2906	struct l2cap_conf_opt opt = ptr;
2907	int len;
2908
2909	len = L2CAP_CONF_OPT_SIZE + opt->len;
2910	*ptr += len;
2911
2912	*type = opt->type;
2913	*olen = opt->len;
2914
2915	switch (opt->len) {
2916	case 1:
2917	val = ((u8 *) opt->val);
2918	break;
2919
2920	case 2:
2921	*val = get_unaligned_le16(opt->val);
2922	break;
2923
2924	case 4:
2925	*val = get_unaligned_le32(opt->val);
2926	break;
2927
2928	default:
2929	*val = (unsigned long) opt->val;
2930	break;
2931	}
2932
2933	BT_DBG("type 0x%2.2x len %u val 0x%lx", type, opt->len, val);
2934	return len;
2935	}
2936
2937	static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val)
2938	{
2939	struct l2cap_conf_opt opt = ptr;
2940
2941	BT_DBG("type 0x%2.2x len %u val 0x%lx", type, len, val);
2942
2943	opt->type = type;
2944	opt->len = len;
2945
2946	switch (len) {
2947	case 1:
2948	((u8 ) opt->val) = val;
2949	break;
2950
2951	case 2:
2952	put_unaligned_le16(val, opt->val);
2953	break;
2954
2955	case 4:
2956	put_unaligned_le32(val, opt->val);
2957	break;
2958
2959	default:
2960	memcpy(opt->val, (void *) val, len);
2961	break;
2962	}
2963
2964	*ptr += L2CAP_CONF_OPT_SIZE + len;
2965	}
2966
2967	static void l2cap_add_opt_efs(void *ptr, struct l2cap_chan chan)
2968	{
2969	struct l2cap_conf_efs efs;
2970
2971	switch (chan->mode) {
2972	case L2CAP_MODE_ERTM:
2973	efs.id = chan->local_id;
2974	efs.stype = chan->local_stype;
2975	efs.msdu = cpu_to_le16(chan->local_msdu);
2976	efs.sdu_itime = cpu_to_le32(chan->local_sdu_itime);
2977	efs.acc_lat = __constant_cpu_to_le32(L2CAP_DEFAULT_ACC_LAT);
2978	efs.flush_to = __constant_cpu_to_le32(L2CAP_EFS_DEFAULT_FLUSH_TO);
2979	break;
2980
2981	case L2CAP_MODE_STREAMING:
2982	efs.id = 1;
2983	efs.stype = L2CAP_SERV_BESTEFFORT;
2984	efs.msdu = cpu_to_le16(chan->local_msdu);
2985	efs.sdu_itime = cpu_to_le32(chan->local_sdu_itime);
2986	efs.acc_lat = 0;
2987	efs.flush_to = 0;
2988	break;
2989
2990	default:
2991	return;
2992	}
2993
2994	l2cap_add_conf_opt(ptr, L2CAP_CONF_EFS, sizeof(efs),
2995	(unsigned long) &efs);
2996	}
2997
2998	static void l2cap_ack_timeout(struct work_struct *work)
2999	{
3000	struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
3001	ack_timer.work);
3002	u16 frames_to_ack;
3003
3004	BT_DBG("chan %p", chan);
3005
3006	l2cap_chan_lock(chan);
3007
3008	frames_to_ack = __seq_offset(chan, chan->buffer_seq,
3009	chan->last_acked_seq);
3010
3011	if (frames_to_ack)
3012	l2cap_send_rr_or_rnr(chan, 0);
3013
3014	l2cap_chan_unlock(chan);
3015	l2cap_chan_put(chan);
3016	}
3017
3018	int l2cap_ertm_init(struct l2cap_chan *chan)
3019	{
3020	int err;
3021
3022	chan->next_tx_seq = 0;
3023	chan->expected_tx_seq = 0;
3024	chan->expected_ack_seq = 0;
3025	chan->unacked_frames = 0;
3026	chan->buffer_seq = 0;
3027	chan->frames_sent = 0;
3028	chan->last_acked_seq = 0;
3029	chan->sdu = NULL;
3030	chan->sdu_last_frag = NULL;
3031	chan->sdu_len = 0;
3032
3033	skb_queue_head_init(&chan->tx_q);
3034
3035	chan->local_amp_id = AMP_ID_BREDR;
3036	chan->move_id = AMP_ID_BREDR;
3037	chan->move_state = L2CAP_MOVE_STABLE;
3038	chan->move_role = L2CAP_MOVE_ROLE_NONE;
3039
3040	if (chan->mode != L2CAP_MODE_ERTM)
3041	return 0;
3042
3043	chan->rx_state = L2CAP_RX_STATE_RECV;
3044	chan->tx_state = L2CAP_TX_STATE_XMIT;
3045
3046	INIT_DELAYED_WORK(&chan->retrans_timer, l2cap_retrans_timeout);
3047	INIT_DELAYED_WORK(&chan->monitor_timer, l2cap_monitor_timeout);
3048	INIT_DELAYED_WORK(&chan->ack_timer, l2cap_ack_timeout);
3049
3050	skb_queue_head_init(&chan->srej_q);
3051
3052	err = l2cap_seq_list_init(&chan->srej_list, chan->tx_win);
3053	if (err < 0)
3054	return err;
3055
3056	err = l2cap_seq_list_init(&chan->retrans_list, chan->remote_tx_win);
3057	if (err < 0)
3058	l2cap_seq_list_free(&chan->srej_list);
3059
3060	return err;
3061	}
3062
3063	static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask)
3064	{
3065	switch (mode) {
3066	case L2CAP_MODE_STREAMING:
3067	case L2CAP_MODE_ERTM:
3068	if (l2cap_mode_supported(mode, remote_feat_mask))
3069	return mode;
3070	/* fall through */
3071	default:
3072	return L2CAP_MODE_BASIC;
3073	}
3074	}
3075
3076	static inline bool __l2cap_ews_supported(struct l2cap_conn *conn)
3077	{
3078	return conn->hs_enabled && conn->feat_mask & L2CAP_FEAT_EXT_WINDOW;
3079	}
3080
3081	static inline bool __l2cap_efs_supported(struct l2cap_conn *conn)
3082	{
3083	return conn->hs_enabled && conn->feat_mask & L2CAP_FEAT_EXT_FLOW;
3084	}
3085
3086	static void __l2cap_set_ertm_timeouts(struct l2cap_chan *chan,
3087	struct l2cap_conf_rfc *rfc)
3088	{
3089	if (chan->local_amp_id != AMP_ID_BREDR && chan->hs_hcon) {
3090	u64 ertm_to = chan->hs_hcon->hdev->amp_be_flush_to;
3091
3092	/* Class 1 devices have must have ERTM timeouts
3093	* exceeding the Link Supervision Timeout. The
3094	* default Link Supervision Timeout for AMP
3095	* controllers is 10 seconds.
3096	*
3097	* Class 1 devices use 0xffffffff for their
3098	* best-effort flush timeout, so the clamping logic
3099	* will result in a timeout that meets the above
3100	* requirement. ERTM timeouts are 16-bit values, so
3101	* the maximum timeout is 65.535 seconds.
3102	*/
3103
3104	/* Convert timeout to milliseconds and round */
3105	ertm_to = DIV_ROUND_UP_ULL(ertm_to, 1000);
3106
3107	/* This is the recommended formula for class 2 devices
3108	* that start ERTM timers when packets are sent to the
3109	* controller.
3110	*/
3111	ertm_to = 3 * ertm_to + 500;
3112
3113	if (ertm_to > 0xffff)
3114	ertm_to = 0xffff;
3115
3116	rfc->retrans_timeout = cpu_to_le16((u16) ertm_to);
3117	rfc->monitor_timeout = rfc->retrans_timeout;
3118	} else {
3119	rfc->retrans_timeout = __constant_cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO);
3120	rfc->monitor_timeout = __constant_cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO);
3121	}
3122	}
3123
3124	static inline void l2cap_txwin_setup(struct l2cap_chan *chan)
3125	{
3126	if (chan->tx_win > L2CAP_DEFAULT_TX_WINDOW &&
3127	__l2cap_ews_supported(chan->conn)) {
3128	/* use extended control field */
3129	set_bit(FLAG_EXT_CTRL, &chan->flags);
3130	chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
3131	} else {
3132	chan->tx_win = min_t(u16, chan->tx_win,
3133	L2CAP_DEFAULT_TX_WINDOW);
3134	chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW;
3135	}
3136	chan->ack_win = chan->tx_win;
3137	}
3138
3139	static int l2cap_build_conf_req(struct l2cap_chan chan, void data)
3140	{
3141	struct l2cap_conf_req *req = data;
3142	struct l2cap_conf_rfc rfc = { .mode = chan->mode };
3143	void *ptr = req->data;
3144	u16 size;
3145
3146	BT_DBG("chan %p", chan);
3147
3148	if (chan->num_conf_req \|\| chan->num_conf_rsp)
3149	goto done;
3150
3151	switch (chan->mode) {
3152	case L2CAP_MODE_STREAMING:
3153	case L2CAP_MODE_ERTM:
3154	if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state))
3155	break;
3156
3157	if (__l2cap_efs_supported(chan->conn))
3158	set_bit(FLAG_EFS_ENABLE, &chan->flags);
3159
3160	/* fall through */
3161	default:
3162	chan->mode = l2cap_select_mode(rfc.mode, chan->conn->feat_mask);
3163	break;
3164	}
3165
3166	done:
3167	if (chan->imtu != L2CAP_DEFAULT_MTU)
3168	l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu);
3169
3170	switch (chan->mode) {
3171	case L2CAP_MODE_BASIC:
3172	if (!(chan->conn->feat_mask & L2CAP_FEAT_ERTM) &&
3173	!(chan->conn->feat_mask & L2CAP_FEAT_STREAMING))
3174	break;
3175
3176	rfc.mode = L2CAP_MODE_BASIC;
3177	rfc.txwin_size = 0;
3178	rfc.max_transmit = 0;
3179	rfc.retrans_timeout = 0;
3180	rfc.monitor_timeout = 0;
3181	rfc.max_pdu_size = 0;
3182
3183	l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3184	(unsigned long) &rfc);
3185	break;
3186
3187	case L2CAP_MODE_ERTM:
3188	rfc.mode = L2CAP_MODE_ERTM;
3189	rfc.max_transmit = chan->max_tx;
3190
3191	__l2cap_set_ertm_timeouts(chan, &rfc);
3192
3193	size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
3194	L2CAP_EXT_HDR_SIZE - L2CAP_SDULEN_SIZE -
3195	L2CAP_FCS_SIZE);
3196	rfc.max_pdu_size = cpu_to_le16(size);
3197
3198	l2cap_txwin_setup(chan);
3199
3200	rfc.txwin_size = min_t(u16, chan->tx_win,
3201	L2CAP_DEFAULT_TX_WINDOW);
3202
3203	l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3204	(unsigned long) &rfc);
3205
3206	if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
3207	l2cap_add_opt_efs(&ptr, chan);
3208
3209	if (test_bit(FLAG_EXT_CTRL, &chan->flags))
3210	l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
3211	chan->tx_win);
3212
3213	if (chan->conn->feat_mask & L2CAP_FEAT_FCS)
3214	if (chan->fcs == L2CAP_FCS_NONE \|\|
3215	test_bit(CONF_RECV_NO_FCS, &chan->conf_state)) {
3216	chan->fcs = L2CAP_FCS_NONE;
3217	l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1,
3218	chan->fcs);
3219	}
3220	break;
3221
3222	case L2CAP_MODE_STREAMING:
3223	l2cap_txwin_setup(chan);
3224	rfc.mode = L2CAP_MODE_STREAMING;
3225	rfc.txwin_size = 0;
3226	rfc.max_transmit = 0;
3227	rfc.retrans_timeout = 0;
3228	rfc.monitor_timeout = 0;
3229
3230	size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
3231	L2CAP_EXT_HDR_SIZE - L2CAP_SDULEN_SIZE -
3232	L2CAP_FCS_SIZE);
3233	rfc.max_pdu_size = cpu_to_le16(size);
3234
3235	l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3236	(unsigned long) &rfc);
3237
3238	if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
3239	l2cap_add_opt_efs(&ptr, chan);
3240
3241	if (chan->conn->feat_mask & L2CAP_FEAT_FCS)
3242	if (chan->fcs == L2CAP_FCS_NONE \|\|
3243	test_bit(CONF_RECV_NO_FCS, &chan->conf_state)) {
3244	chan->fcs = L2CAP_FCS_NONE;
3245	l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1,
3246	chan->fcs);
3247	}
3248	break;
3249	}
3250
3251	req->dcid = cpu_to_le16(chan->dcid);
3252	req->flags = __constant_cpu_to_le16(0);
3253
3254	return ptr - data;
3255	}
3256
3257	static int l2cap_parse_conf_req(struct l2cap_chan chan, void data)
3258	{
3259	struct l2cap_conf_rsp *rsp = data;
3260	void *ptr = rsp->data;
3261	void *req = chan->conf_req;
3262	int len = chan->conf_len;
3263	int type, hint, olen;
3264	unsigned long val;
3265	struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
3266	struct l2cap_conf_efs efs;
3267	u8 remote_efs = 0;
3268	u16 mtu = L2CAP_DEFAULT_MTU;
3269	u16 result = L2CAP_CONF_SUCCESS;
3270	u16 size;
3271
3272	BT_DBG("chan %p", chan);
3273
3274	while (len >= L2CAP_CONF_OPT_SIZE) {
3275	len -= l2cap_get_conf_opt(&req, &type, &olen, &val);
3276
3277	hint = type & L2CAP_CONF_HINT;
3278	type &= L2CAP_CONF_MASK;
3279
3280	switch (type) {
3281	case L2CAP_CONF_MTU:
3282	mtu = val;
3283	break;
3284
3285	case L2CAP_CONF_FLUSH_TO:
3286	chan->flush_to = val;
3287	break;
3288
3289	case L2CAP_CONF_QOS:
3290	break;
3291
3292	case L2CAP_CONF_RFC:
3293	if (olen == sizeof(rfc))
3294	memcpy(&rfc, (void *) val, olen);
3295	break;
3296
3297	case L2CAP_CONF_FCS:
3298	if (val == L2CAP_FCS_NONE)
3299	set_bit(CONF_RECV_NO_FCS, &chan->conf_state);
3300	break;
3301
3302	case L2CAP_CONF_EFS:
3303	remote_efs = 1;
3304	if (olen == sizeof(efs))
3305	memcpy(&efs, (void *) val, olen);
3306	break;
3307
3308	case L2CAP_CONF_EWS:
3309	if (!chan->conn->hs_enabled)
3310	return -ECONNREFUSED;
3311
3312	set_bit(FLAG_EXT_CTRL, &chan->flags);
3313	set_bit(CONF_EWS_RECV, &chan->conf_state);
3314	chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
3315	chan->remote_tx_win = val;
3316	break;
3317
3318	default:
3319	if (hint)
3320	break;
3321
3322	result = L2CAP_CONF_UNKNOWN;
3323	((u8 ) ptr++) = type;
3324	break;
3325	}
3326	}
3327
3328	if (chan->num_conf_rsp \|\| chan->num_conf_req > 1)
3329	goto done;
3330
3331	switch (chan->mode) {
3332	case L2CAP_MODE_STREAMING:
3333	case L2CAP_MODE_ERTM:
3334	if (!test_bit(CONF_STATE2_DEVICE, &chan->conf_state)) {
3335	chan->mode = l2cap_select_mode(rfc.mode,
3336	chan->conn->feat_mask);
3337	break;
3338	}
3339
3340	if (remote_efs) {
3341	if (__l2cap_efs_supported(chan->conn))
3342	set_bit(FLAG_EFS_ENABLE, &chan->flags);
3343	else
3344	return -ECONNREFUSED;
3345	}
3346
3347	if (chan->mode != rfc.mode)
3348	return -ECONNREFUSED;
3349
3350	break;
3351	}
3352
3353	done:
3354	if (chan->mode != rfc.mode) {
3355	result = L2CAP_CONF_UNACCEPT;
3356	rfc.mode = chan->mode;
3357
3358	if (chan->num_conf_rsp == 1)
3359	return -ECONNREFUSED;
3360
3361	l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3362	(unsigned long) &rfc);
3363	}
3364
3365	if (result == L2CAP_CONF_SUCCESS) {
3366	/* Configure output options and let the other side know
3367	* which ones we don't like. */
3368
3369	if (mtu < L2CAP_DEFAULT_MIN_MTU)
3370	result = L2CAP_CONF_UNACCEPT;
3371	else {
3372	chan->omtu = mtu;
3373	set_bit(CONF_MTU_DONE, &chan->conf_state);
3374	}
3375	l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->omtu);
3376
3377	if (remote_efs) {
3378	if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
3379	efs.stype != L2CAP_SERV_NOTRAFIC &&
3380	efs.stype != chan->local_stype) {
3381
3382	result = L2CAP_CONF_UNACCEPT;
3383
3384	if (chan->num_conf_req >= 1)
3385	return -ECONNREFUSED;
3386
3387	l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
3388	sizeof(efs),
3389	(unsigned long) &efs);
3390	} else {
3391	/* Send PENDING Conf Rsp */
3392	result = L2CAP_CONF_PENDING;
3393	set_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
3394	}
3395	}
3396
3397	switch (rfc.mode) {
3398	case L2CAP_MODE_BASIC:
3399	chan->fcs = L2CAP_FCS_NONE;
3400	set_bit(CONF_MODE_DONE, &chan->conf_state);
3401	break;
3402
3403	case L2CAP_MODE_ERTM:
3404	if (!test_bit(CONF_EWS_RECV, &chan->conf_state))
3405	chan->remote_tx_win = rfc.txwin_size;
3406	else
3407	rfc.txwin_size = L2CAP_DEFAULT_TX_WINDOW;
3408
3409	chan->remote_max_tx = rfc.max_transmit;
3410
3411	size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
3412	chan->conn->mtu - L2CAP_EXT_HDR_SIZE -
3413	L2CAP_SDULEN_SIZE - L2CAP_FCS_SIZE);
3414	rfc.max_pdu_size = cpu_to_le16(size);
3415	chan->remote_mps = size;
3416
3417	__l2cap_set_ertm_timeouts(chan, &rfc);
3418
3419	set_bit(CONF_MODE_DONE, &chan->conf_state);
3420
3421	l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
3422	sizeof(rfc), (unsigned long) &rfc);
3423
3424	if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
3425	chan->remote_id = efs.id;
3426	chan->remote_stype = efs.stype;
3427	chan->remote_msdu = le16_to_cpu(efs.msdu);
3428	chan->remote_flush_to =
3429	le32_to_cpu(efs.flush_to);
3430	chan->remote_acc_lat =
3431	le32_to_cpu(efs.acc_lat);
3432	chan->remote_sdu_itime =
3433	le32_to_cpu(efs.sdu_itime);
3434	l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
3435	sizeof(efs),
3436	(unsigned long) &efs);
3437	}
3438	break;
3439
3440	case L2CAP_MODE_STREAMING:
3441	size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
3442	chan->conn->mtu - L2CAP_EXT_HDR_SIZE -
3443	L2CAP_SDULEN_SIZE - L2CAP_FCS_SIZE);
3444	rfc.max_pdu_size = cpu_to_le16(size);
3445	chan->remote_mps = size;
3446
3447	set_bit(CONF_MODE_DONE, &chan->conf_state);
3448
3449	l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3450	(unsigned long) &rfc);
3451
3452	break;
3453
3454	default:
3455	result = L2CAP_CONF_UNACCEPT;
3456
3457	memset(&rfc, 0, sizeof(rfc));
3458	rfc.mode = chan->mode;
3459	}
3460
3461	if (result == L2CAP_CONF_SUCCESS)
3462	set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
3463	}
3464	rsp->scid = cpu_to_le16(chan->dcid);
3465	rsp->result = cpu_to_le16(result);
3466	rsp->flags = __constant_cpu_to_le16(0);
3467
3468	return ptr - data;
3469	}
3470
3471	static int l2cap_parse_conf_rsp(struct l2cap_chan chan, void rsp, int len,
3472	void data, u16 result)
3473	{
3474	struct l2cap_conf_req *req = data;
3475	void *ptr = req->data;
3476	int type, olen;
3477	unsigned long val;
3478	struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
3479	struct l2cap_conf_efs efs;
3480
3481	BT_DBG("chan %p, rsp %p, len %d, req %p", chan, rsp, len, data);
3482
3483	while (len >= L2CAP_CONF_OPT_SIZE) {
3484	len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
3485
3486	switch (type) {
3487	case L2CAP_CONF_MTU:
3488	if (val < L2CAP_DEFAULT_MIN_MTU) {
3489	*result = L2CAP_CONF_UNACCEPT;
3490	chan->imtu = L2CAP_DEFAULT_MIN_MTU;
3491	} else
3492	chan->imtu = val;
3493	l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu);
3494	break;
3495
3496	case L2CAP_CONF_FLUSH_TO:
3497	chan->flush_to = val;
3498	l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
3499	2, chan->flush_to);
3500	break;
3501
3502	case L2CAP_CONF_RFC:
3503	if (olen == sizeof(rfc))
3504	memcpy(&rfc, (void *)val, olen);
3505
3506	if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state) &&
3507	rfc.mode != chan->mode)
3508	return -ECONNREFUSED;
3509
3510	chan->fcs = 0;
3511
3512	l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
3513	sizeof(rfc), (unsigned long) &rfc);
3514	break;
3515
3516	case L2CAP_CONF_EWS:
3517	chan->ack_win = min_t(u16, val, chan->ack_win);
3518	l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
3519	chan->tx_win);
3520	break;
3521
3522	case L2CAP_CONF_EFS:
3523	if (olen == sizeof(efs))
3524	memcpy(&efs, (void *)val, olen);
3525
3526	if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
3527	efs.stype != L2CAP_SERV_NOTRAFIC &&
3528	efs.stype != chan->local_stype)
3529	return -ECONNREFUSED;
3530
3531	l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS, sizeof(efs),
3532	(unsigned long) &efs);
3533	break;
3534
3535	case L2CAP_CONF_FCS:
3536	if (*result == L2CAP_CONF_PENDING)
3537	if (val == L2CAP_FCS_NONE)
3538	set_bit(CONF_RECV_NO_FCS,
3539	&chan->conf_state);
3540	break;
3541	}
3542	}
3543
3544	if (chan->mode == L2CAP_MODE_BASIC && chan->mode != rfc.mode)
3545	return -ECONNREFUSED;
3546
3547	chan->mode = rfc.mode;
3548
3549	if (result == L2CAP_CONF_SUCCESS \|\| result == L2CAP_CONF_PENDING) {
3550	switch (rfc.mode) {
3551	case L2CAP_MODE_ERTM:
3552	chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
3553	chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
3554	chan->mps = le16_to_cpu(rfc.max_pdu_size);
3555	if (!test_bit(FLAG_EXT_CTRL, &chan->flags))
3556	chan->ack_win = min_t(u16, chan->ack_win,
3557	rfc.txwin_size);
3558
3559	if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
3560	chan->local_msdu = le16_to_cpu(efs.msdu);
3561	chan->local_sdu_itime =
3562	le32_to_cpu(efs.sdu_itime);
3563	chan->local_acc_lat = le32_to_cpu(efs.acc_lat);
3564	chan->local_flush_to =
3565	le32_to_cpu(efs.flush_to);
3566	}
3567	break;
3568
3569	case L2CAP_MODE_STREAMING:
3570	chan->mps = le16_to_cpu(rfc.max_pdu_size);
3571	}
3572	}
3573
3574	req->dcid = cpu_to_le16(chan->dcid);
3575	req->flags = __constant_cpu_to_le16(0);
3576
3577	return ptr - data;
3578	}
3579
3580	static int l2cap_build_conf_rsp(struct l2cap_chan chan, void data,
3581	u16 result, u16 flags)
3582	{
3583	struct l2cap_conf_rsp *rsp = data;
3584	void *ptr = rsp->data;
3585
3586	BT_DBG("chan %p", chan);
3587
3588	rsp->scid = cpu_to_le16(chan->dcid);
3589	rsp->result = cpu_to_le16(result);
3590	rsp->flags = cpu_to_le16(flags);
3591
3592	return ptr - data;
3593	}
3594
3595	void __l2cap_connect_rsp_defer(struct l2cap_chan *chan)
3596	{
3597	struct l2cap_conn_rsp rsp;
3598	struct l2cap_conn *conn = chan->conn;
3599	u8 buf[128];
3600	u8 rsp_code;
3601
3602	rsp.scid = cpu_to_le16(chan->dcid);
3603	rsp.dcid = cpu_to_le16(chan->scid);
3604	rsp.result = __constant_cpu_to_le16(L2CAP_CR_SUCCESS);
3605	rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
3606
3607	if (chan->hs_hcon)
3608	rsp_code = L2CAP_CREATE_CHAN_RSP;
3609	else
3610	rsp_code = L2CAP_CONN_RSP;
3611
3612	BT_DBG("chan %p rsp_code %u", chan, rsp_code);
3613
3614	l2cap_send_cmd(conn, chan->ident, rsp_code, sizeof(rsp), &rsp);
3615
3616	if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
3617	return;
3618
3619	l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3620	l2cap_build_conf_req(chan, buf), buf);
3621	chan->num_conf_req++;
3622	}
3623
3624	static void l2cap_conf_rfc_get(struct l2cap_chan chan, void rsp, int len)
3625	{
3626	int type, olen;
3627	unsigned long val;
3628	/* Use sane default values in case a misbehaving remote device
3629	* did not send an RFC or extended window size option.
3630	*/
3631	u16 txwin_ext = chan->ack_win;
3632	struct l2cap_conf_rfc rfc = {
3633	.mode = chan->mode,
3634	.retrans_timeout = __constant_cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO),
3635	.monitor_timeout = __constant_cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO),
3636	.max_pdu_size = cpu_to_le16(chan->imtu),
3637	.txwin_size = min_t(u16, chan->ack_win, L2CAP_DEFAULT_TX_WINDOW),
3638	};
3639
3640	BT_DBG("chan %p, rsp %p, len %d", chan, rsp, len);
3641
3642	if ((chan->mode != L2CAP_MODE_ERTM) && (chan->mode != L2CAP_MODE_STREAMING))
3643	return;
3644
3645	while (len >= L2CAP_CONF_OPT_SIZE) {
3646	len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
3647
3648	switch (type) {
3649	case L2CAP_CONF_RFC:
3650	if (olen == sizeof(rfc))
3651	memcpy(&rfc, (void *)val, olen);
3652	break;
3653	case L2CAP_CONF_EWS:
3654	txwin_ext = val;
3655	break;
3656	}
3657	}
3658
3659	switch (rfc.mode) {
3660	case L2CAP_MODE_ERTM:
3661	chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
3662	chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
3663	chan->mps = le16_to_cpu(rfc.max_pdu_size);
3664	if (test_bit(FLAG_EXT_CTRL, &chan->flags))
3665	chan->ack_win = min_t(u16, chan->ack_win, txwin_ext);
3666	else
3667	chan->ack_win = min_t(u16, chan->ack_win,
3668	rfc.txwin_size);
3669	break;
3670	case L2CAP_MODE_STREAMING:
3671	chan->mps = le16_to_cpu(rfc.max_pdu_size);
3672	}
3673	}
3674
3675	static inline int l2cap_command_rej(struct l2cap_conn *conn,
3676	struct l2cap_cmd_hdr *cmd, u16 cmd_len,
3677	u8 *data)
3678	{
3679	struct l2cap_cmd_rej_unk rej = (struct l2cap_cmd_rej_unk ) data;
3680
3681	if (cmd_len < sizeof(*rej))
3682	return -EPROTO;
3683
3684	if (rej->reason != L2CAP_REJ_NOT_UNDERSTOOD)
3685	return 0;
3686
3687	if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
3688	cmd->ident == conn->info_ident) {
3689	cancel_delayed_work(&conn->info_timer);
3690
3691	conn->info_state \|= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3692	conn->info_ident = 0;
3693
3694	l2cap_conn_start(conn);
3695	}
3696
3697	return 0;
3698	}
3699
3700	static struct l2cap_chan l2cap_connect(struct l2cap_conn conn,
3701	struct l2cap_cmd_hdr *cmd,
3702	u8 *data, u8 rsp_code, u8 amp_id)
3703	{
3704	struct l2cap_conn_req req = (struct l2cap_conn_req ) data;
3705	struct l2cap_conn_rsp rsp;
3706	struct l2cap_chan chan = NULL, pchan;
3707	int result, status = L2CAP_CS_NO_INFO;
3708
3709	u16 dcid = 0, scid = __le16_to_cpu(req->scid);
3710	__le16 psm = req->psm;
3711
3712	BT_DBG("psm 0x%2.2x scid 0x%4.4x", __le16_to_cpu(psm), scid);
3713
3714	/* Check if we have socket listening on psm */
3715	pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, &conn->hcon->src,
3716	&conn->hcon->dst);
3717	if (!pchan) {
3718	result = L2CAP_CR_BAD_PSM;
3719	goto sendresp;
3720	}
3721
3722	mutex_lock(&conn->chan_lock);
3723	l2cap_chan_lock(pchan);
3724
3725	/* Check if the ACL is secure enough (if not SDP) */
3726	if (psm != __constant_cpu_to_le16(L2CAP_PSM_SDP) &&
3727	!hci_conn_check_link_mode(conn->hcon)) {
3728	conn->disc_reason = HCI_ERROR_AUTH_FAILURE;
3729	result = L2CAP_CR_SEC_BLOCK;
3730	goto response;
3731	}
3732
3733	result = L2CAP_CR_NO_MEM;
3734
3735	/* Check if we already have channel with that dcid */
3736	if (__l2cap_get_chan_by_dcid(conn, scid))
3737	goto response;
3738
3739	chan = pchan->ops->new_connection(pchan);
3740	if (!chan)
3741	goto response;
3742
3743	/* For certain devices (ex: HID mouse), support for authentication,
3744	* pairing and bonding is optional. For such devices, inorder to avoid
3745	* the ACL alive for too long after L2CAP disconnection, reset the ACL
3746	* disc_timeout back to HCI_DISCONN_TIMEOUT during L2CAP connect.
3747	*/
3748	conn->hcon->disc_timeout = HCI_DISCONN_TIMEOUT;
3749
3750	bacpy(&chan->src, &conn->hcon->src);
3751	bacpy(&chan->dst, &conn->hcon->dst);
3752	chan->src_type = bdaddr_type(conn->hcon, conn->hcon->src_type);
3753	chan->dst_type = bdaddr_type(conn->hcon, conn->hcon->dst_type);
3754	chan->psm = psm;
3755	chan->dcid = scid;
3756	chan->local_amp_id = amp_id;
3757
3758	__l2cap_chan_add(conn, chan);
3759
3760	dcid = chan->scid;
3761
3762	__set_chan_timer(chan, chan->ops->get_sndtimeo(chan));
3763
3764	chan->ident = cmd->ident;
3765
3766	if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
3767	if (l2cap_chan_check_security(chan)) {
3768	if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
3769	l2cap_state_change(chan, BT_CONNECT2);
3770	result = L2CAP_CR_PEND;
3771	status = L2CAP_CS_AUTHOR_PEND;
3772	chan->ops->defer(chan);
3773	} else {
3774	/* Force pending result for AMP controllers.
3775	* The connection will succeed after the
3776	* physical link is up.
3777	*/
3778	if (amp_id == AMP_ID_BREDR) {
3779	l2cap_state_change(chan, BT_CONFIG);
3780	result = L2CAP_CR_SUCCESS;
3781	} else {
3782	l2cap_state_change(chan, BT_CONNECT2);
3783	result = L2CAP_CR_PEND;
3784	}
3785	status = L2CAP_CS_NO_INFO;
3786	}
3787	} else {
3788	l2cap_state_change(chan, BT_CONNECT2);
3789	result = L2CAP_CR_PEND;
3790	status = L2CAP_CS_AUTHEN_PEND;
3791	}
3792	} else {
3793	l2cap_state_change(chan, BT_CONNECT2);
3794	result = L2CAP_CR_PEND;
3795	status = L2CAP_CS_NO_INFO;
3796	}
3797
3798	response:
3799	l2cap_chan_unlock(pchan);
3800	mutex_unlock(&conn->chan_lock);
3801
3802	sendresp:
3803	rsp.scid = cpu_to_le16(scid);
3804	rsp.dcid = cpu_to_le16(dcid);
3805	rsp.result = cpu_to_le16(result);
3806	rsp.status = cpu_to_le16(status);
3807	l2cap_send_cmd(conn, cmd->ident, rsp_code, sizeof(rsp), &rsp);
3808
3809	if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
3810	struct l2cap_info_req info;
3811	info.type = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
3812
3813	conn->info_state \|= L2CAP_INFO_FEAT_MASK_REQ_SENT;
3814	conn->info_ident = l2cap_get_ident(conn);
3815
3816	schedule_delayed_work(&conn->info_timer, L2CAP_INFO_TIMEOUT);
3817
3818	l2cap_send_cmd(conn, conn->info_ident, L2CAP_INFO_REQ,
3819	sizeof(info), &info);
3820	}
3821
3822	if (chan && !test_bit(CONF_REQ_SENT, &chan->conf_state) &&
3823	result == L2CAP_CR_SUCCESS) {
3824	u8 buf[128];
3825	set_bit(CONF_REQ_SENT, &chan->conf_state);
3826	l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3827	l2cap_build_conf_req(chan, buf), buf);
3828	chan->num_conf_req++;
3829	}
3830
3831	return chan;
3832	}
3833
3834	static int l2cap_connect_req(struct l2cap_conn *conn,
3835	struct l2cap_cmd_hdr cmd, u16 cmd_len, u8 data)
3836	{
3837	struct hci_dev *hdev = conn->hcon->hdev;
3838	struct hci_conn *hcon = conn->hcon;
3839
3840	if (cmd_len < sizeof(struct l2cap_conn_req))
3841	return -EPROTO;
3842
3843	hci_dev_lock(hdev);
3844	if (test_bit(HCI_MGMT, &hdev->dev_flags) &&
3845	!test_and_set_bit(HCI_CONN_MGMT_CONNECTED, &hcon->flags))
3846	mgmt_device_connected(hdev, &hcon->dst, hcon->type,
3847	hcon->dst_type, 0, NULL, 0,
3848	hcon->dev_class);
3849	hci_dev_unlock(hdev);
3850
3851	l2cap_connect(conn, cmd, data, L2CAP_CONN_RSP, 0);
3852	return 0;
3853	}
3854
3855	static int l2cap_connect_create_rsp(struct l2cap_conn *conn,
3856	struct l2cap_cmd_hdr *cmd, u16 cmd_len,
3857	u8 *data)
3858	{
3859	struct l2cap_conn_rsp rsp = (struct l2cap_conn_rsp ) data;
3860	u16 scid, dcid, result, status;
3861	struct l2cap_chan *chan;
3862	u8 req[128];
3863	int err;
3864
3865	if (cmd_len < sizeof(*rsp))
3866	return -EPROTO;
3867
3868	scid = __le16_to_cpu(rsp->scid);
3869	dcid = __le16_to_cpu(rsp->dcid);
3870	result = __le16_to_cpu(rsp->result);
3871	status = __le16_to_cpu(rsp->status);
3872
3873	BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x",
3874	dcid, scid, result, status);
3875
3876	mutex_lock(&conn->chan_lock);
3877
3878	if (scid) {
3879	chan = __l2cap_get_chan_by_scid(conn, scid);
3880	if (!chan) {
3881	err = -EBADSLT;
3882	goto unlock;
3883	}
3884	} else {
3885	chan = __l2cap_get_chan_by_ident(conn, cmd->ident);
3886	if (!chan) {
3887	err = -EBADSLT;
3888	goto unlock;
3889	}
3890	}
3891
3892	err = 0;
3893
3894	l2cap_chan_lock(chan);
3895
3896	switch (result) {
3897	case L2CAP_CR_SUCCESS:
3898	l2cap_state_change(chan, BT_CONFIG);
3899	chan->ident = 0;
3900	chan->dcid = dcid;
3901	clear_bit(CONF_CONNECT_PEND, &chan->conf_state);
3902
3903	if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
3904	break;
3905
3906	l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3907	l2cap_build_conf_req(chan, req), req);
3908	chan->num_conf_req++;
3909	break;
3910
3911	case L2CAP_CR_PEND:
3912	set_bit(CONF_CONNECT_PEND, &chan->conf_state);
3913	break;
3914
3915	default:
3916	l2cap_chan_del(chan, ECONNREFUSED);
3917	break;
3918	}
3919
3920	l2cap_chan_unlock(chan);
3921
3922	unlock:
3923	mutex_unlock(&conn->chan_lock);
3924
3925	return err;
3926	}
3927
3928	static inline void set_default_fcs(struct l2cap_chan *chan)
3929	{
3930	/* FCS is enabled only in ERTM or streaming mode, if one or both
3931	* sides request it.
3932	*/
3933	if (chan->mode != L2CAP_MODE_ERTM && chan->mode != L2CAP_MODE_STREAMING)
3934	chan->fcs = L2CAP_FCS_NONE;
3935	else if (!test_bit(CONF_RECV_NO_FCS, &chan->conf_state))
3936	chan->fcs = L2CAP_FCS_CRC16;
3937	}
3938
3939	static void l2cap_send_efs_conf_rsp(struct l2cap_chan chan, void data,
3940	u8 ident, u16 flags)
3941	{
3942	struct l2cap_conn *conn = chan->conn;
3943
3944	BT_DBG("conn %p chan %p ident %d flags 0x%4.4x", conn, chan, ident,
3945	flags);
3946
3947	clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
3948	set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
3949
3950	l2cap_send_cmd(conn, ident, L2CAP_CONF_RSP,
3951	l2cap_build_conf_rsp(chan, data,
3952	L2CAP_CONF_SUCCESS, flags), data);
3953	}
3954
3955	static void cmd_reject_invalid_cid(struct l2cap_conn *conn, u8 ident,
3956	u16 scid, u16 dcid)
3957	{
3958	struct l2cap_cmd_rej_cid rej;
3959
3960	rej.reason = __constant_cpu_to_le16(L2CAP_REJ_INVALID_CID);
3961	rej.scid = __cpu_to_le16(scid);
3962	rej.dcid = __cpu_to_le16(dcid);
3963
3964	l2cap_send_cmd(conn, ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
3965	}
3966
3967	static inline int l2cap_config_req(struct l2cap_conn *conn,
3968	struct l2cap_cmd_hdr *cmd, u16 cmd_len,
3969	u8 *data)
3970	{
3971	struct l2cap_conf_req req = (struct l2cap_conf_req ) data;
3972	u16 dcid, flags;
3973	u8 rsp[64];
3974	struct l2cap_chan *chan;
3975	int len, err = 0;
3976
3977	if (cmd_len < sizeof(*req))
3978	return -EPROTO;
3979
3980	dcid = __le16_to_cpu(req->dcid);
3981	flags = __le16_to_cpu(req->flags);
3982
3983	BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
3984
3985	chan = l2cap_get_chan_by_scid(conn, dcid);
3986	if (!chan) {
3987	cmd_reject_invalid_cid(conn, cmd->ident, dcid, 0);
3988	return 0;
3989	}
3990
3991	if (chan->state != BT_CONFIG && chan->state != BT_CONNECT2) {
3992	cmd_reject_invalid_cid(conn, cmd->ident, chan->scid,
3993	chan->dcid);
3994	goto unlock;
3995	}
3996
3997	/* Reject if config buffer is too small. */
3998	len = cmd_len - sizeof(*req);
3999	if (chan->conf_len + len > sizeof(chan->conf_req)) {
4000	l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
4001	l2cap_build_conf_rsp(chan, rsp,
4002	L2CAP_CONF_REJECT, flags), rsp);
4003	goto unlock;
4004	}
4005
4006	/* Store config. */
4007	memcpy(chan->conf_req + chan->conf_len, req->data, len);
4008	chan->conf_len += len;
4009
4010	if (flags & L2CAP_CONF_FLAG_CONTINUATION) {
4011	/* Incomplete config. Send empty response. */
4012	l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
4013	l2cap_build_conf_rsp(chan, rsp,
4014	L2CAP_CONF_SUCCESS, flags), rsp);
4015	goto unlock;
4016	}
4017
4018	/* Complete config. */
4019	len = l2cap_parse_conf_req(chan, rsp);
4020	if (len < 0) {
4021	l2cap_send_disconn_req(chan, ECONNRESET);
4022	goto unlock;
4023	}
4024
4025	chan->ident = cmd->ident;
4026	l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
4027	chan->num_conf_rsp++;
4028
4029	/* Reset config buffer. */
4030	chan->conf_len = 0;
4031
4032	if (!test_bit(CONF_OUTPUT_DONE, &chan->conf_state))
4033	goto unlock;
4034
4035	if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
4036	set_default_fcs(chan);
4037
4038	if (chan->mode == L2CAP_MODE_ERTM \|\|
4039	chan->mode == L2CAP_MODE_STREAMING)
4040	err = l2cap_ertm_init(chan);
4041
4042	if (err < 0)
4043	l2cap_send_disconn_req(chan, -err);
4044	else
4045	l2cap_chan_ready(chan);
4046
4047	goto unlock;
4048	}
4049
4050	if (!test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) {
4051	u8 buf[64];
4052	l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
4053	l2cap_build_conf_req(chan, buf), buf);
4054	chan->num_conf_req++;
4055	}
4056
4057	/* Got Conf Rsp PENDING from remote side and asume we sent
4058	Conf Rsp PENDING in the code above */
4059	if (test_bit(CONF_REM_CONF_PEND, &chan->conf_state) &&
4060	test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
4061
4062	/* check compatibility */
4063
4064	/* Send rsp for BR/EDR channel */
4065	if (!chan->hs_hcon)
4066	l2cap_send_efs_conf_rsp(chan, rsp, cmd->ident, flags);
4067	else
4068	chan->ident = cmd->ident;
4069	}
4070
4071	unlock:
4072	l2cap_chan_unlock(chan);
4073	return err;
4074	}
4075
4076	static inline int l2cap_config_rsp(struct l2cap_conn *conn,
4077	struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4078	u8 *data)
4079	{
4080	struct l2cap_conf_rsp rsp = (struct l2cap_conf_rsp )data;
4081	u16 scid, flags, result;
4082	struct l2cap_chan *chan;
4083	int len = cmd_len - sizeof(*rsp);
4084	int err = 0;
4085
4086	if (cmd_len < sizeof(*rsp))
4087	return -EPROTO;
4088
4089	scid = __le16_to_cpu(rsp->scid);
4090	flags = __le16_to_cpu(rsp->flags);
4091	result = __le16_to_cpu(rsp->result);
4092
4093	BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x len %d", scid, flags,
4094	result, len);
4095
4096	chan = l2cap_get_chan_by_scid(conn, scid);
4097	if (!chan)
4098	return 0;
4099
4100	switch (result) {
4101	case L2CAP_CONF_SUCCESS:
4102	l2cap_conf_rfc_get(chan, rsp->data, len);
4103	clear_bit(CONF_REM_CONF_PEND, &chan->conf_state);
4104	break;
4105
4106	case L2CAP_CONF_PENDING:
4107	set_bit(CONF_REM_CONF_PEND, &chan->conf_state);
4108
4109	if (test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
4110	char buf[64];
4111
4112	len = l2cap_parse_conf_rsp(chan, rsp->data, len,
4113	buf, &result);
4114	if (len < 0) {
4115	l2cap_send_disconn_req(chan, ECONNRESET);
4116	goto done;
4117	}
4118
4119	if (!chan->hs_hcon) {
4120	l2cap_send_efs_conf_rsp(chan, buf, cmd->ident,
4121	0);
4122	} else {
4123	if (l2cap_check_efs(chan)) {
4124	amp_create_logical_link(chan);
4125	chan->ident = cmd->ident;
4126	}
4127	}
4128	}
4129	goto done;
4130
4131	case L2CAP_CONF_UNACCEPT:
4132	if (chan->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
4133	char req[64];
4134
4135	if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) {
4136	l2cap_send_disconn_req(chan, ECONNRESET);
4137	goto done;
4138	}
4139
4140	/* throw out any old stored conf requests */
4141	result = L2CAP_CONF_SUCCESS;
4142	len = l2cap_parse_conf_rsp(chan, rsp->data, len,
4143	req, &result);
4144	if (len < 0) {
4145	l2cap_send_disconn_req(chan, ECONNRESET);
4146	goto done;
4147	}
4148
4149	l2cap_send_cmd(conn, l2cap_get_ident(conn),
4150	L2CAP_CONF_REQ, len, req);
4151	chan->num_conf_req++;
4152	if (result != L2CAP_CONF_SUCCESS)
4153	goto done;
4154	break;
4155	}
4156
4157	default:
4158	l2cap_chan_set_err(chan, ECONNRESET);
4159
4160	__set_chan_timer(chan, L2CAP_DISC_REJ_TIMEOUT);
4161	l2cap_send_disconn_req(chan, ECONNRESET);
4162	goto done;
4163	}
4164
4165	if (flags & L2CAP_CONF_FLAG_CONTINUATION)
4166	goto done;
4167
4168	set_bit(CONF_INPUT_DONE, &chan->conf_state);
4169
4170	if (test_bit(CONF_OUTPUT_DONE, &chan->conf_state)) {
4171	set_default_fcs(chan);
4172
4173	if (chan->mode == L2CAP_MODE_ERTM \|\|
4174	chan->mode == L2CAP_MODE_STREAMING)
4175	err = l2cap_ertm_init(chan);
4176
4177	if (err < 0)
4178	l2cap_send_disconn_req(chan, -err);
4179	else
4180	l2cap_chan_ready(chan);
4181	}
4182
4183	done:
4184	l2cap_chan_unlock(chan);
4185	return err;
4186	}
4187
4188	static inline int l2cap_disconnect_req(struct l2cap_conn *conn,
4189	struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4190	u8 *data)
4191	{
4192	struct l2cap_disconn_req req = (struct l2cap_disconn_req ) data;
4193	struct l2cap_disconn_rsp rsp;
4194	u16 dcid, scid;
4195	struct l2cap_chan *chan;
4196
4197	if (cmd_len != sizeof(*req))
4198	return -EPROTO;
4199
4200	scid = __le16_to_cpu(req->scid);
4201	dcid = __le16_to_cpu(req->dcid);
4202
4203	BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
4204
4205	mutex_lock(&conn->chan_lock);
4206
4207	chan = __l2cap_get_chan_by_scid(conn, dcid);
4208	if (!chan) {
4209	mutex_unlock(&conn->chan_lock);
4210	cmd_reject_invalid_cid(conn, cmd->ident, dcid, scid);
4211	return 0;
4212	}
4213
4214	l2cap_chan_lock(chan);
4215
4216	rsp.dcid = cpu_to_le16(chan->scid);
4217	rsp.scid = cpu_to_le16(chan->dcid);
4218	l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
4219
4220	chan->ops->set_shutdown(chan);
4221
4222	l2cap_chan_hold(chan);
4223	l2cap_chan_del(chan, ECONNRESET);
4224
4225	l2cap_chan_unlock(chan);
4226
4227	chan->ops->close(chan);
4228	l2cap_chan_put(chan);
4229
4230	mutex_unlock(&conn->chan_lock);
4231
4232	return 0;
4233	}
4234
4235	static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn,
4236	struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4237	u8 *data)
4238	{
4239	struct l2cap_disconn_rsp rsp = (struct l2cap_disconn_rsp ) data;
4240	u16 dcid, scid;
4241	struct l2cap_chan *chan;
4242
4243	if (cmd_len != sizeof(*rsp))
4244	return -EPROTO;
4245
4246	scid = __le16_to_cpu(rsp->scid);
4247	dcid = __le16_to_cpu(rsp->dcid);
4248
4249	BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
4250
4251	mutex_lock(&conn->chan_lock);
4252
4253	chan = __l2cap_get_chan_by_scid(conn, scid);
4254	if (!chan) {
4255	mutex_unlock(&conn->chan_lock);
4256	return 0;
4257	}
4258
4259	l2cap_chan_lock(chan);
4260
4261	l2cap_chan_hold(chan);
4262	l2cap_chan_del(chan, 0);
4263
4264	l2cap_chan_unlock(chan);
4265
4266	chan->ops->close(chan);
4267	l2cap_chan_put(chan);
4268
4269	mutex_unlock(&conn->chan_lock);
4270
4271	return 0;
4272	}
4273
4274	static inline int l2cap_information_req(struct l2cap_conn *conn,
4275	struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4276	u8 *data)
4277	{
4278	struct l2cap_info_req req = (struct l2cap_info_req ) data;
4279	u16 type;
4280
4281	if (cmd_len != sizeof(*req))
4282	return -EPROTO;
4283
4284	type = __le16_to_cpu(req->type);
4285
4286	BT_DBG("type 0x%4.4x", type);
4287
4288	if (type == L2CAP_IT_FEAT_MASK) {
4289	u8 buf[8];
4290	u32 feat_mask = l2cap_feat_mask;
4291	struct l2cap_info_rsp rsp = (struct l2cap_info_rsp ) buf;
4292	rsp->type = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
4293	rsp->result = __constant_cpu_to_le16(L2CAP_IR_SUCCESS);
4294	if (!disable_ertm)
4295	feat_mask \|= L2CAP_FEAT_ERTM \| L2CAP_FEAT_STREAMING
4296	\| L2CAP_FEAT_FCS;
4297	if (conn->hs_enabled)
4298	feat_mask \|= L2CAP_FEAT_EXT_FLOW
4299	\| L2CAP_FEAT_EXT_WINDOW;
4300
4301	put_unaligned_le32(feat_mask, rsp->data);
4302	l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf),
4303	buf);
4304	} else if (type == L2CAP_IT_FIXED_CHAN) {
4305	u8 buf[12];
4306	struct l2cap_info_rsp rsp = (struct l2cap_info_rsp ) buf;
4307
4308	if (conn->hs_enabled)
4309	l2cap_fixed_chan[0] \|= L2CAP_FC_A2MP;
4310	else
4311	l2cap_fixed_chan[0] &= ~L2CAP_FC_A2MP;
4312
4313	rsp->type = __constant_cpu_to_le16(L2CAP_IT_FIXED_CHAN);
4314	rsp->result = __constant_cpu_to_le16(L2CAP_IR_SUCCESS);
4315	memcpy(rsp->data, l2cap_fixed_chan, sizeof(l2cap_fixed_chan));
4316	l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf),
4317	buf);
4318	} else {
4319	struct l2cap_info_rsp rsp;
4320	rsp.type = cpu_to_le16(type);
4321	rsp.result = __constant_cpu_to_le16(L2CAP_IR_NOTSUPP);
4322	l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(rsp),
4323	&rsp);
4324	}
4325
4326	return 0;
4327	}
4328
4329	static inline int l2cap_information_rsp(struct l2cap_conn *conn,
4330	struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4331	u8 *data)
4332	{
4333	struct l2cap_info_rsp rsp = (struct l2cap_info_rsp ) data;
4334	u16 type, result;
4335
4336	if (cmd_len < sizeof(*rsp))
4337	return -EPROTO;
4338
4339	type = __le16_to_cpu(rsp->type);
4340	result = __le16_to_cpu(rsp->result);
4341
4342	BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
4343
4344	/* L2CAP Info req/rsp are unbound to channels, add extra checks */
4345	if (cmd->ident != conn->info_ident \|\|
4346	conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)
4347	return 0;
4348
4349	cancel_delayed_work(&conn->info_timer);
4350
4351	if (result != L2CAP_IR_SUCCESS) {
4352	conn->info_state \|= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4353	conn->info_ident = 0;
4354
4355	l2cap_conn_start(conn);
4356
4357	return 0;
4358	}
4359
4360	switch (type) {
4361	case L2CAP_IT_FEAT_MASK:
4362	conn->feat_mask = get_unaligned_le32(rsp->data);
4363
4364	if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
4365	struct l2cap_info_req req;
4366	req.type = __constant_cpu_to_le16(L2CAP_IT_FIXED_CHAN);
4367
4368	conn->info_ident = l2cap_get_ident(conn);
4369
4370	l2cap_send_cmd(conn, conn->info_ident,
4371	L2CAP_INFO_REQ, sizeof(req), &req);
4372	} else {
4373	conn->info_state \|= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4374	conn->info_ident = 0;
4375
4376	l2cap_conn_start(conn);
4377	}
4378	break;
4379
4380	case L2CAP_IT_FIXED_CHAN:
4381	conn->fixed_chan_mask = rsp->data[0];
4382	conn->info_state \|= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4383	conn->info_ident = 0;
4384
4385	l2cap_conn_start(conn);
4386	break;
4387	}
4388
4389	return 0;
4390	}
4391
4392	static int l2cap_create_channel_req(struct l2cap_conn *conn,
4393	struct l2cap_cmd_hdr *cmd,
4394	u16 cmd_len, void *data)
4395	{
4396	struct l2cap_create_chan_req *req = data;
4397	struct l2cap_create_chan_rsp rsp;
4398	struct l2cap_chan *chan;
4399	struct hci_dev *hdev;
4400	u16 psm, scid;
4401
4402	if (cmd_len != sizeof(*req))
4403	return -EPROTO;
4404
4405	if (!conn->hs_enabled)
4406	return -EINVAL;
4407
4408	psm = le16_to_cpu(req->psm);
4409	scid = le16_to_cpu(req->scid);
4410
4411	BT_DBG("psm 0x%2.2x, scid 0x%4.4x, amp_id %d", psm, scid, req->amp_id);
4412
4413	/* For controller id 0 make BR/EDR connection */
4414	if (req->amp_id == AMP_ID_BREDR) {
4415	l2cap_connect(conn, cmd, data, L2CAP_CREATE_CHAN_RSP,
4416	req->amp_id);
4417	return 0;
4418	}
4419
4420	/* Validate AMP controller id */
4421	hdev = hci_dev_get(req->amp_id);
4422	if (!hdev)
4423	goto error;
4424
4425	if (hdev->dev_type != HCI_AMP \|\| !test_bit(HCI_UP, &hdev->flags)) {
4426	hci_dev_put(hdev);
4427	goto error;
4428	}
4429
4430	chan = l2cap_connect(conn, cmd, data, L2CAP_CREATE_CHAN_RSP,
4431	req->amp_id);
4432	if (chan) {
4433	struct amp_mgr *mgr = conn->hcon->amp_mgr;
4434	struct hci_conn *hs_hcon;
4435
4436	hs_hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK,
4437	&conn->hcon->dst);
4438	if (!hs_hcon) {
4439	hci_dev_put(hdev);
4440	cmd_reject_invalid_cid(conn, cmd->ident, chan->scid,
4441	chan->dcid);
4442	return 0;
4443	}
4444
4445	BT_DBG("mgr %p bredr_chan %p hs_hcon %p", mgr, chan, hs_hcon);
4446
4447	mgr->bredr_chan = chan;
4448	chan->hs_hcon = hs_hcon;
4449	chan->fcs = L2CAP_FCS_NONE;
4450	conn->mtu = hdev->block_mtu;
4451	}
4452
4453	hci_dev_put(hdev);
4454
4455	return 0;
4456
4457	error:
4458	rsp.dcid = 0;
4459	rsp.scid = cpu_to_le16(scid);
4460	rsp.result = __constant_cpu_to_le16(L2CAP_CR_BAD_AMP);
4461	rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
4462
4463	l2cap_send_cmd(conn, cmd->ident, L2CAP_CREATE_CHAN_RSP,
4464	sizeof(rsp), &rsp);
4465
4466	return 0;
4467	}
4468
4469	static void l2cap_send_move_chan_req(struct l2cap_chan *chan, u8 dest_amp_id)
4470	{
4471	struct l2cap_move_chan_req req;
4472	u8 ident;
4473
4474	BT_DBG("chan %p, dest_amp_id %d", chan, dest_amp_id);
4475
4476	ident = l2cap_get_ident(chan->conn);
4477	chan->ident = ident;
4478
4479	req.icid = cpu_to_le16(chan->scid);
4480	req.dest_amp_id = dest_amp_id;
4481
4482	l2cap_send_cmd(chan->conn, ident, L2CAP_MOVE_CHAN_REQ, sizeof(req),
4483	&req);
4484
4485	__set_chan_timer(chan, L2CAP_MOVE_TIMEOUT);
4486	}
4487
4488	static void l2cap_send_move_chan_rsp(struct l2cap_chan *chan, u16 result)
4489	{
4490	struct l2cap_move_chan_rsp rsp;
4491
4492	BT_DBG("chan %p, result 0x%4.4x", chan, result);
4493
4494	rsp.icid = cpu_to_le16(chan->dcid);
4495	rsp.result = cpu_to_le16(result);
4496
4497	l2cap_send_cmd(chan->conn, chan->ident, L2CAP_MOVE_CHAN_RSP,
4498	sizeof(rsp), &rsp);
4499	}
4500
4501	static void l2cap_send_move_chan_cfm(struct l2cap_chan *chan, u16 result)
4502	{
4503	struct l2cap_move_chan_cfm cfm;
4504
4505	BT_DBG("chan %p, result 0x%4.4x", chan, result);
4506
4507	chan->ident = l2cap_get_ident(chan->conn);
4508
4509	cfm.icid = cpu_to_le16(chan->scid);
4510	cfm.result = cpu_to_le16(result);
4511
4512	l2cap_send_cmd(chan->conn, chan->ident, L2CAP_MOVE_CHAN_CFM,
4513	sizeof(cfm), &cfm);
4514
4515	__set_chan_timer(chan, L2CAP_MOVE_TIMEOUT);
4516	}
4517
4518	static void l2cap_send_move_chan_cfm_icid(struct l2cap_conn *conn, u16 icid)
4519	{
4520	struct l2cap_move_chan_cfm cfm;
4521
4522	BT_DBG("conn %p, icid 0x%4.4x", conn, icid);
4523
4524	cfm.icid = cpu_to_le16(icid);
4525	cfm.result = __constant_cpu_to_le16(L2CAP_MC_UNCONFIRMED);
4526
4527	l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_MOVE_CHAN_CFM,
4528	sizeof(cfm), &cfm);
4529	}
4530
4531	static void l2cap_send_move_chan_cfm_rsp(struct l2cap_conn *conn, u8 ident,
4532	u16 icid)
4533	{
4534	struct l2cap_move_chan_cfm_rsp rsp;
4535
4536	BT_DBG("icid 0x%4.4x", icid);
4537
4538	rsp.icid = cpu_to_le16(icid);
4539	l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM_RSP, sizeof(rsp), &rsp);
4540	}
4541
4542	static void __release_logical_link(struct l2cap_chan *chan)
4543	{
4544	chan->hs_hchan = NULL;
4545	chan->hs_hcon = NULL;
4546
4547	/* Placeholder - release the logical link */
4548	}
4549
4550	static void l2cap_logical_fail(struct l2cap_chan *chan)
4551	{
4552	/* Logical link setup failed */
4553	if (chan->state != BT_CONNECTED) {
4554	/* Create channel failure, disconnect */
4555	l2cap_send_disconn_req(chan, ECONNRESET);
4556	return;
4557	}
4558
4559	switch (chan->move_role) {
4560	case L2CAP_MOVE_ROLE_RESPONDER:
4561	l2cap_move_done(chan);
4562	l2cap_send_move_chan_rsp(chan, L2CAP_MR_NOT_SUPP);
4563	break;
4564	case L2CAP_MOVE_ROLE_INITIATOR:
4565	if (chan->move_state == L2CAP_MOVE_WAIT_LOGICAL_COMP \|\|
4566	chan->move_state == L2CAP_MOVE_WAIT_LOGICAL_CFM) {
4567	/* Remote has only sent pending or
4568	* success responses, clean up
4569	*/
4570	l2cap_move_done(chan);
4571	}
4572
4573	/* Other amp move states imply that the move
4574	* has already aborted
4575	*/
4576	l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
4577	break;
4578	}
4579	}
4580
4581	static void l2cap_logical_finish_create(struct l2cap_chan *chan,
4582	struct hci_chan *hchan)
4583	{
4584	struct l2cap_conf_rsp rsp;
4585
4586	chan->hs_hchan = hchan;
4587	chan->hs_hcon->l2cap_data = chan->conn;
4588
4589	l2cap_send_efs_conf_rsp(chan, &rsp, chan->ident, 0);
4590
4591	if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
4592	int err;
4593
4594	set_default_fcs(chan);
4595
4596	err = l2cap_ertm_init(chan);
4597	if (err < 0)
4598	l2cap_send_disconn_req(chan, -err);
4599	else
4600	l2cap_chan_ready(chan);
4601	}
4602	}
4603
4604	static void l2cap_logical_finish_move(struct l2cap_chan *chan,
4605	struct hci_chan *hchan)
4606	{
4607	chan->hs_hcon = hchan->conn;
4608	chan->hs_hcon->l2cap_data = chan->conn;
4609
4610	BT_DBG("move_state %d", chan->move_state);
4611
4612	switch (chan->move_state) {
4613	case L2CAP_MOVE_WAIT_LOGICAL_COMP:
4614	/* Move confirm will be sent after a success
4615	* response is received
4616	*/
4617	chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
4618	break;
4619	case L2CAP_MOVE_WAIT_LOGICAL_CFM:
4620	if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
4621	chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
4622	} else if (chan->move_role == L2CAP_MOVE_ROLE_INITIATOR) {
4623	chan->move_state = L2CAP_MOVE_WAIT_CONFIRM_RSP;
4624	l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
4625	} else if (chan->move_role == L2CAP_MOVE_ROLE_RESPONDER) {
4626	chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
4627	l2cap_send_move_chan_rsp(chan, L2CAP_MR_SUCCESS);
4628	}
4629	break;
4630	default:
4631	/* Move was not in expected state, free the channel */
4632	__release_logical_link(chan);
4633
4634	chan->move_state = L2CAP_MOVE_STABLE;
4635	}
4636	}
4637
4638	/* Call with chan locked */
4639	void l2cap_logical_cfm(struct l2cap_chan chan, struct hci_chan hchan,
4640	u8 status)
4641	{
4642	BT_DBG("chan %p, hchan %p, status %d", chan, hchan, status);
4643
4644	if (status) {
4645	l2cap_logical_fail(chan);
4646	__release_logical_link(chan);
4647	return;
4648	}
4649
4650	if (chan->state != BT_CONNECTED) {
4651	/* Ignore logical link if channel is on BR/EDR */
4652	if (chan->local_amp_id != AMP_ID_BREDR)
4653	l2cap_logical_finish_create(chan, hchan);
4654	} else {
4655	l2cap_logical_finish_move(chan, hchan);
4656	}
4657	}
4658
4659	void l2cap_move_start(struct l2cap_chan *chan)
4660	{
4661	BT_DBG("chan %p", chan);
4662
4663	if (chan->local_amp_id == AMP_ID_BREDR) {
4664	if (chan->chan_policy != BT_CHANNEL_POLICY_AMP_PREFERRED)
4665	return;
4666	chan->move_role = L2CAP_MOVE_ROLE_INITIATOR;
4667	chan->move_state = L2CAP_MOVE_WAIT_PREPARE;
4668	/* Placeholder - start physical link setup */
4669	} else {
4670	chan->move_role = L2CAP_MOVE_ROLE_INITIATOR;
4671	chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
4672	chan->move_id = 0;
4673	l2cap_move_setup(chan);
4674	l2cap_send_move_chan_req(chan, 0);
4675	}
4676	}
4677
4678	static void l2cap_do_create(struct l2cap_chan *chan, int result,
4679	u8 local_amp_id, u8 remote_amp_id)
4680	{
4681	BT_DBG("chan %p state %s %u -> %u", chan, state_to_string(chan->state),
4682	local_amp_id, remote_amp_id);
4683
4684	chan->fcs = L2CAP_FCS_NONE;
4685
4686	/* Outgoing channel on AMP */
4687	if (chan->state == BT_CONNECT) {
4688	if (result == L2CAP_CR_SUCCESS) {
4689	chan->local_amp_id = local_amp_id;
4690	l2cap_send_create_chan_req(chan, remote_amp_id);
4691	} else {
4692	/* Revert to BR/EDR connect */
4693	l2cap_send_conn_req(chan);
4694	}
4695
4696	return;
4697	}
4698
4699	/* Incoming channel on AMP */
4700	if (__l2cap_no_conn_pending(chan)) {
4701	struct l2cap_conn_rsp rsp;
4702	char buf[128];
4703	rsp.scid = cpu_to_le16(chan->dcid);
4704	rsp.dcid = cpu_to_le16(chan->scid);
4705
4706	if (result == L2CAP_CR_SUCCESS) {
4707	/* Send successful response */
4708	rsp.result = __constant_cpu_to_le16(L2CAP_CR_SUCCESS);
4709	rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
4710	} else {
4711	/* Send negative response */
4712	rsp.result = __constant_cpu_to_le16(L2CAP_CR_NO_MEM);
4713	rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
4714	}
4715
4716	l2cap_send_cmd(chan->conn, chan->ident, L2CAP_CREATE_CHAN_RSP,
4717	sizeof(rsp), &rsp);
4718
4719	if (result == L2CAP_CR_SUCCESS) {
4720	l2cap_state_change(chan, BT_CONFIG);
4721	set_bit(CONF_REQ_SENT, &chan->conf_state);
4722	l2cap_send_cmd(chan->conn, l2cap_get_ident(chan->conn),
4723	L2CAP_CONF_REQ,
4724	l2cap_build_conf_req(chan, buf), buf);
4725	chan->num_conf_req++;
4726	}
4727	}
4728	}
4729
4730	static void l2cap_do_move_initiate(struct l2cap_chan *chan, u8 local_amp_id,
4731	u8 remote_amp_id)
4732	{
4733	l2cap_move_setup(chan);
4734	chan->move_id = local_amp_id;
4735	chan->move_state = L2CAP_MOVE_WAIT_RSP;
4736
4737	l2cap_send_move_chan_req(chan, remote_amp_id);
4738	}
4739
4740	static void l2cap_do_move_respond(struct l2cap_chan *chan, int result)
4741	{
4742	struct hci_chan *hchan = NULL;
4743
4744	/* Placeholder - get hci_chan for logical link */
4745
4746	if (hchan) {
4747	if (hchan->state == BT_CONNECTED) {
4748	/* Logical link is ready to go */
4749	chan->hs_hcon = hchan->conn;
4750	chan->hs_hcon->l2cap_data = chan->conn;
4751	chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
4752	l2cap_send_move_chan_rsp(chan, L2CAP_MR_SUCCESS);
4753
4754	l2cap_logical_cfm(chan, hchan, L2CAP_MR_SUCCESS);
4755	} else {
4756	/* Wait for logical link to be ready */
4757	chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
4758	}
4759	} else {
4760	/* Logical link not available */
4761	l2cap_send_move_chan_rsp(chan, L2CAP_MR_NOT_ALLOWED);
4762	}
4763	}
4764
4765	static void l2cap_do_move_cancel(struct l2cap_chan *chan, int result)
4766	{
4767	if (chan->move_role == L2CAP_MOVE_ROLE_RESPONDER) {
4768	u8 rsp_result;
4769	if (result == -EINVAL)
4770	rsp_result = L2CAP_MR_BAD_ID;
4771	else
4772	rsp_result = L2CAP_MR_NOT_ALLOWED;
4773
4774	l2cap_send_move_chan_rsp(chan, rsp_result);
4775	}
4776
4777	chan->move_role = L2CAP_MOVE_ROLE_NONE;
4778	chan->move_state = L2CAP_MOVE_STABLE;
4779
4780	/* Restart data transmission */
4781	l2cap_ertm_send(chan);
4782	}
4783
4784	/* Invoke with locked chan */
4785	void __l2cap_physical_cfm(struct l2cap_chan *chan, int result)
4786	{
4787	u8 local_amp_id = chan->local_amp_id;
4788	u8 remote_amp_id = chan->remote_amp_id;
4789
4790	BT_DBG("chan %p, result %d, local_amp_id %d, remote_amp_id %d",
4791	chan, result, local_amp_id, remote_amp_id);
4792
4793	if (chan->state == BT_DISCONN \|\| chan->state == BT_CLOSED) {
4794	l2cap_chan_unlock(chan);
4795	return;
4796	}
4797
4798	if (chan->state != BT_CONNECTED) {
4799	l2cap_do_create(chan, result, local_amp_id, remote_amp_id);
4800	} else if (result != L2CAP_MR_SUCCESS) {
4801	l2cap_do_move_cancel(chan, result);
4802	} else {
4803	switch (chan->move_role) {
4804	case L2CAP_MOVE_ROLE_INITIATOR:
4805	l2cap_do_move_initiate(chan, local_amp_id,
4806	remote_amp_id);
4807	break;
4808	case L2CAP_MOVE_ROLE_RESPONDER:
4809	l2cap_do_move_respond(chan, result);
4810	break;
4811	default:
4812	l2cap_do_move_cancel(chan, result);
4813	break;
4814	}
4815	}
4816	}
4817
4818	static inline int l2cap_move_channel_req(struct l2cap_conn *conn,
4819	struct l2cap_cmd_hdr *cmd,
4820	u16 cmd_len, void *data)
4821	{
4822	struct l2cap_move_chan_req *req = data;
4823	struct l2cap_move_chan_rsp rsp;
4824	struct l2cap_chan *chan;
4825	u16 icid = 0;
4826	u16 result = L2CAP_MR_NOT_ALLOWED;
4827
4828	if (cmd_len != sizeof(*req))
4829	return -EPROTO;
4830
4831	icid = le16_to_cpu(req->icid);
4832
4833	BT_DBG("icid 0x%4.4x, dest_amp_id %d", icid, req->dest_amp_id);
4834
4835	if (!conn->hs_enabled)
4836	return -EINVAL;
4837
4838	chan = l2cap_get_chan_by_dcid(conn, icid);
4839	if (!chan) {
4840	rsp.icid = cpu_to_le16(icid);
4841	rsp.result = __constant_cpu_to_le16(L2CAP_MR_NOT_ALLOWED);
4842	l2cap_send_cmd(conn, cmd->ident, L2CAP_MOVE_CHAN_RSP,
4843	sizeof(rsp), &rsp);
4844	return 0;
4845	}
4846
4847	chan->ident = cmd->ident;
4848
4849	if (chan->scid < L2CAP_CID_DYN_START \|\|
4850	chan->chan_policy == BT_CHANNEL_POLICY_BREDR_ONLY \|\|
4851	(chan->mode != L2CAP_MODE_ERTM &&
4852	chan->mode != L2CAP_MODE_STREAMING)) {
4853	result = L2CAP_MR_NOT_ALLOWED;
4854	goto send_move_response;
4855	}
4856
4857	if (chan->local_amp_id == req->dest_amp_id) {
4858	result = L2CAP_MR_SAME_ID;
4859	goto send_move_response;
4860	}
4861
4862	if (req->dest_amp_id != AMP_ID_BREDR) {
4863	struct hci_dev *hdev;
4864	hdev = hci_dev_get(req->dest_amp_id);
4865	if (!hdev \|\| hdev->dev_type != HCI_AMP \|\|
4866	!test_bit(HCI_UP, &hdev->flags)) {
4867	if (hdev)
4868	hci_dev_put(hdev);
4869
4870	result = L2CAP_MR_BAD_ID;
4871	goto send_move_response;
4872	}
4873	hci_dev_put(hdev);
4874	}
4875
4876	/* Detect a move collision. Only send a collision response
4877	* if this side has "lost", otherwise proceed with the move.
4878	* The winner has the larger bd_addr.
4879	*/
4880	if ((__chan_is_moving(chan) \|\|
4881	chan->move_role != L2CAP_MOVE_ROLE_NONE) &&
4882	bacmp(&conn->hcon->src, &conn->hcon->dst) > 0) {
4883	result = L2CAP_MR_COLLISION;
4884	goto send_move_response;
4885	}
4886
4887	chan->move_role = L2CAP_MOVE_ROLE_RESPONDER;
4888	l2cap_move_setup(chan);
4889	chan->move_id = req->dest_amp_id;
4890	icid = chan->dcid;
4891
4892	if (req->dest_amp_id == AMP_ID_BREDR) {
4893	/* Moving to BR/EDR */
4894	if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
4895	chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
4896	result = L2CAP_MR_PEND;
4897	} else {
4898	chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
4899	result = L2CAP_MR_SUCCESS;
4900	}
4901	} else {
4902	chan->move_state = L2CAP_MOVE_WAIT_PREPARE;
4903	/* Placeholder - uncomment when amp functions are available */
4904	/amp_accept_physical(chan, req->dest_amp_id);/
4905	result = L2CAP_MR_PEND;
4906	}
4907
4908	send_move_response:
4909	l2cap_send_move_chan_rsp(chan, result);
4910
4911	l2cap_chan_unlock(chan);
4912
4913	return 0;
4914	}
4915
4916	static void l2cap_move_continue(struct l2cap_conn *conn, u16 icid, u16 result)
4917	{
4918	struct l2cap_chan *chan;
4919	struct hci_chan *hchan = NULL;
4920
4921	chan = l2cap_get_chan_by_scid(conn, icid);
4922	if (!chan) {
4923	l2cap_send_move_chan_cfm_icid(conn, icid);
4924	return;
4925	}
4926
4927	__clear_chan_timer(chan);
4928	if (result == L2CAP_MR_PEND)
4929	__set_chan_timer(chan, L2CAP_MOVE_ERTX_TIMEOUT);
4930
4931	switch (chan->move_state) {
4932	case L2CAP_MOVE_WAIT_LOGICAL_COMP:
4933	/* Move confirm will be sent when logical link
4934	* is complete.
4935	*/
4936	chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
4937	break;
4938	case L2CAP_MOVE_WAIT_RSP_SUCCESS:
4939	if (result == L2CAP_MR_PEND) {
4940	break;
4941	} else if (test_bit(CONN_LOCAL_BUSY,
4942	&chan->conn_state)) {
4943	chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
4944	} else {
4945	/* Logical link is up or moving to BR/EDR,
4946	* proceed with move
4947	*/
4948	chan->move_state = L2CAP_MOVE_WAIT_CONFIRM_RSP;
4949	l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
4950	}
4951	break;
4952	case L2CAP_MOVE_WAIT_RSP:
4953	/* Moving to AMP */
4954	if (result == L2CAP_MR_SUCCESS) {
4955	/* Remote is ready, send confirm immediately
4956	* after logical link is ready
4957	*/
4958	chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
4959	} else {
4960	/* Both logical link and move success
4961	* are required to confirm
4962	*/
4963	chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_COMP;
4964	}
4965
4966	/* Placeholder - get hci_chan for logical link */
4967	if (!hchan) {
4968	/* Logical link not available */
4969	l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
4970	break;
4971	}
4972
4973	/* If the logical link is not yet connected, do not
4974	* send confirmation.
4975	*/
4976	if (hchan->state != BT_CONNECTED)
4977	break;
4978
4979	/* Logical link is already ready to go */
4980
4981	chan->hs_hcon = hchan->conn;
4982	chan->hs_hcon->l2cap_data = chan->conn;
4983
4984	if (result == L2CAP_MR_SUCCESS) {
4985	/* Can confirm now */
4986	l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
4987	} else {
4988	/* Now only need move success
4989	* to confirm
4990	*/
4991	chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
4992	}
4993
4994	l2cap_logical_cfm(chan, hchan, L2CAP_MR_SUCCESS);
4995	break;
4996	default:
4997	/* Any other amp move state means the move failed. */
4998	chan->move_id = chan->local_amp_id;
4999	l2cap_move_done(chan);
5000	l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
5001	}
5002
5003	l2cap_chan_unlock(chan);
5004	}
5005
5006	static void l2cap_move_fail(struct l2cap_conn *conn, u8 ident, u16 icid,
5007	u16 result)
5008	{
5009	struct l2cap_chan *chan;
5010
5011	chan = l2cap_get_chan_by_ident(conn, ident);
5012	if (!chan) {
5013	/* Could not locate channel, icid is best guess */
5014	l2cap_send_move_chan_cfm_icid(conn, icid);
5015	return;
5016	}
5017
5018	__clear_chan_timer(chan);
5019
5020	if (chan->move_role == L2CAP_MOVE_ROLE_INITIATOR) {
5021	if (result == L2CAP_MR_COLLISION) {
5022	chan->move_role = L2CAP_MOVE_ROLE_RESPONDER;
5023	} else {
5024	/* Cleanup - cancel move */
5025	chan->move_id = chan->local_amp_id;
5026	l2cap_move_done(chan);
5027	}
5028	}
5029
5030	l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
5031
5032	l2cap_chan_unlock(chan);
5033	}
5034
5035	static int l2cap_move_channel_rsp(struct l2cap_conn *conn,
5036	struct l2cap_cmd_hdr *cmd,
5037	u16 cmd_len, void *data)
5038	{
5039	struct l2cap_move_chan_rsp *rsp = data;
5040	u16 icid, result;
5041
5042	if (cmd_len != sizeof(*rsp))
5043	return -EPROTO;
5044
5045	icid = le16_to_cpu(rsp->icid);
5046	result = le16_to_cpu(rsp->result);
5047
5048	BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result);
5049
5050	if (result == L2CAP_MR_SUCCESS \|\| result == L2CAP_MR_PEND)
5051	l2cap_move_continue(conn, icid, result);
5052	else
5053	l2cap_move_fail(conn, cmd->ident, icid, result);
5054
5055	return 0;
5056	}
5057
5058	static int l2cap_move_channel_confirm(struct l2cap_conn *conn,
5059	struct l2cap_cmd_hdr *cmd,
5060	u16 cmd_len, void *data)
5061	{
5062	struct l2cap_move_chan_cfm *cfm = data;
5063	struct l2cap_chan *chan;
5064	u16 icid, result;
5065
5066	if (cmd_len != sizeof(*cfm))
5067	return -EPROTO;
5068
5069	icid = le16_to_cpu(cfm->icid);
5070	result = le16_to_cpu(cfm->result);
5071
5072	BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result);
5073
5074	chan = l2cap_get_chan_by_dcid(conn, icid);
5075	if (!chan) {
5076	/* Spec requires a response even if the icid was not found */
5077	l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid);
5078	return 0;
5079	}
5080
5081	if (chan->move_state == L2CAP_MOVE_WAIT_CONFIRM) {
5082	if (result == L2CAP_MC_CONFIRMED) {
5083	chan->local_amp_id = chan->move_id;
5084	if (chan->local_amp_id == AMP_ID_BREDR)
5085	__release_logical_link(chan);
5086	} else {
5087	chan->move_id = chan->local_amp_id;
5088	}
5089
5090	l2cap_move_done(chan);
5091	}
5092
5093	l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid);
5094
5095	l2cap_chan_unlock(chan);
5096
5097	return 0;
5098	}
5099
5100	static inline int l2cap_move_channel_confirm_rsp(struct l2cap_conn *conn,
5101	struct l2cap_cmd_hdr *cmd,
5102	u16 cmd_len, void *data)
5103	{
5104	struct l2cap_move_chan_cfm_rsp *rsp = data;
5105	struct l2cap_chan *chan;
5106	u16 icid;
5107
5108	if (cmd_len != sizeof(*rsp))
5109	return -EPROTO;
5110
5111	icid = le16_to_cpu(rsp->icid);
5112
5113	BT_DBG("icid 0x%4.4x", icid);
5114
5115	chan = l2cap_get_chan_by_scid(conn, icid);
5116	if (!chan)
5117	return 0;
5118
5119	__clear_chan_timer(chan);
5120
5121	if (chan->move_state == L2CAP_MOVE_WAIT_CONFIRM_RSP) {
5122	chan->local_amp_id = chan->move_id;
5123
5124	if (chan->local_amp_id == AMP_ID_BREDR && chan->hs_hchan)
5125	__release_logical_link(chan);
5126
5127	l2cap_move_done(chan);
5128	}
5129
5130	l2cap_chan_unlock(chan);
5131
5132	return 0;
5133	}
5134
5135	static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency,
5136	u16 to_multiplier)
5137	{
5138	u16 max_latency;
5139
5140	if (min > max \|\| min < 6 \|\| max > 3200)
5141	return -EINVAL;
5142
5143	if (to_multiplier < 10 \|\| to_multiplier > 3200)
5144	return -EINVAL;
5145
5146	if (max >= to_multiplier * 8)
5147	return -EINVAL;
5148
5149	max_latency = (to_multiplier * 8 / max) - 1;
5150	if (latency > 499 \|\| latency > max_latency)
5151	return -EINVAL;
5152
5153	return 0;
5154	}
5155
5156	static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
5157	struct l2cap_cmd_hdr *cmd,
5158	u8 *data)
5159	{
5160	struct hci_conn *hcon = conn->hcon;
5161	struct l2cap_conn_param_update_req *req;
5162	struct l2cap_conn_param_update_rsp rsp;
5163	u16 min, max, latency, to_multiplier, cmd_len;
5164	int err;
5165
5166	if (!(hcon->link_mode & HCI_LM_MASTER))
5167	return -EINVAL;
5168
5169	cmd_len = __le16_to_cpu(cmd->len);
5170	if (cmd_len != sizeof(struct l2cap_conn_param_update_req))
5171	return -EPROTO;
5172
5173	req = (struct l2cap_conn_param_update_req *) data;
5174	min = __le16_to_cpu(req->min);
5175	max = __le16_to_cpu(req->max);
5176	latency = __le16_to_cpu(req->latency);
5177	to_multiplier = __le16_to_cpu(req->to_multiplier);
5178
5179	BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x",
5180	min, max, latency, to_multiplier);
5181
5182	memset(&rsp, 0, sizeof(rsp));
5183
5184	err = l2cap_check_conn_param(min, max, latency, to_multiplier);
5185	if (err)
5186	rsp.result = __constant_cpu_to_le16(L2CAP_CONN_PARAM_REJECTED);
5187	else
5188	rsp.result = __constant_cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED);
5189
5190	l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP,
5191	sizeof(rsp), &rsp);
5192
5193	if (!err)
5194	hci_le_conn_update(hcon, min, max, latency, to_multiplier);
5195
5196	return 0;
5197	}
5198
5199	static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
5200	struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5201	u8 *data)
5202	{
5203	int err = 0;
5204
5205	switch (cmd->code) {
5206	case L2CAP_COMMAND_REJ:
5207	l2cap_command_rej(conn, cmd, cmd_len, data);
5208	break;
5209
5210	case L2CAP_CONN_REQ:
5211	err = l2cap_connect_req(conn, cmd, cmd_len, data);
5212	break;
5213
5214	case L2CAP_CONN_RSP:
5215	case L2CAP_CREATE_CHAN_RSP:
5216	l2cap_connect_create_rsp(conn, cmd, cmd_len, data);
5217	break;
5218
5219	case L2CAP_CONF_REQ:
5220	err = l2cap_config_req(conn, cmd, cmd_len, data);
5221	break;
5222
5223	case L2CAP_CONF_RSP:
5224	l2cap_config_rsp(conn, cmd, cmd_len, data);
5225	break;
5226
5227	case L2CAP_DISCONN_REQ:
5228	err = l2cap_disconnect_req(conn, cmd, cmd_len, data);
5229	break;
5230
5231	case L2CAP_DISCONN_RSP:
5232	l2cap_disconnect_rsp(conn, cmd, cmd_len, data);
5233	break;
5234
5235	case L2CAP_ECHO_REQ:
5236	l2cap_send_cmd(conn, cmd->ident, L2CAP_ECHO_RSP, cmd_len, data);
5237	break;
5238
5239	case L2CAP_ECHO_RSP:
5240	break;
5241
5242	case L2CAP_INFO_REQ:
5243	err = l2cap_information_req(conn, cmd, cmd_len, data);
5244	break;
5245
5246	case L2CAP_INFO_RSP:
5247	l2cap_information_rsp(conn, cmd, cmd_len, data);
5248	break;
5249
5250	case L2CAP_CREATE_CHAN_REQ:
5251	err = l2cap_create_channel_req(conn, cmd, cmd_len, data);
5252	break;
5253
5254	case L2CAP_MOVE_CHAN_REQ:
5255	err = l2cap_move_channel_req(conn, cmd, cmd_len, data);
5256	break;
5257
5258	case L2CAP_MOVE_CHAN_RSP:
5259	l2cap_move_channel_rsp(conn, cmd, cmd_len, data);
5260	break;
5261
5262	case L2CAP_MOVE_CHAN_CFM:
5263	err = l2cap_move_channel_confirm(conn, cmd, cmd_len, data);
5264	break;
5265
5266	case L2CAP_MOVE_CHAN_CFM_RSP:
5267	l2cap_move_channel_confirm_rsp(conn, cmd, cmd_len, data);
5268	break;
5269
5270	default:
5271	BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code);
5272	err = -EINVAL;
5273	break;
5274	}
5275
5276	return err;
5277	}
5278
5279	static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn,
5280	struct l2cap_cmd_hdr cmd, u8 data)
5281	{
5282	switch (cmd->code) {
5283	case L2CAP_COMMAND_REJ:
5284	return 0;
5285
5286	case L2CAP_CONN_PARAM_UPDATE_REQ:
5287	return l2cap_conn_param_update_req(conn, cmd, data);
5288
5289	case L2CAP_CONN_PARAM_UPDATE_RSP:
5290	return 0;
5291
5292	default:
5293	BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code);
5294	return -EINVAL;
5295	}
5296	}
5297
5298	static inline void l2cap_le_sig_channel(struct l2cap_conn *conn,
5299	struct sk_buff *skb)
5300	{
5301	struct hci_conn *hcon = conn->hcon;
5302	struct l2cap_cmd_hdr *cmd;
5303	u16 len;
5304	int err;
5305
5306	if (hcon->type != LE_LINK)
5307	goto drop;
5308
5309	if (skb->len < L2CAP_CMD_HDR_SIZE)
5310	goto drop;
5311
5312	cmd = (void *) skb->data;
5313	skb_pull(skb, L2CAP_CMD_HDR_SIZE);
5314
5315	len = le16_to_cpu(cmd->len);
5316
5317	BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd->code, len, cmd->ident);
5318
5319	if (len != skb->len \|\| !cmd->ident) {
5320	BT_DBG("corrupted command");
5321	goto drop;
5322	}
5323
5324	err = l2cap_le_sig_cmd(conn, cmd, skb->data);
5325	if (err) {
5326	struct l2cap_cmd_rej_unk rej;
5327
5328	BT_ERR("Wrong link type (%d)", err);
5329
5330	rej.reason = __constant_cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
5331	l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ,
5332	sizeof(rej), &rej);
5333	}
5334
5335	drop:
5336	kfree_skb(skb);
5337	}
5338
5339	static inline void l2cap_sig_channel(struct l2cap_conn *conn,
5340	struct sk_buff *skb)
5341	{
5342	struct hci_conn *hcon = conn->hcon;
5343	u8 *data = skb->data;
5344	int len = skb->len;
5345	struct l2cap_cmd_hdr cmd;
5346	int err;
5347
5348	l2cap_raw_recv(conn, skb);
5349
5350	if (hcon->type != ACL_LINK)
5351	goto drop;
5352
5353	while (len >= L2CAP_CMD_HDR_SIZE) {
5354	u16 cmd_len;
5355	memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
5356	data += L2CAP_CMD_HDR_SIZE;
5357	len -= L2CAP_CMD_HDR_SIZE;
5358
5359	cmd_len = le16_to_cpu(cmd.len);
5360
5361	BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len,
5362	cmd.ident);
5363
5364	if (cmd_len > len \|\| !cmd.ident) {
5365	BT_DBG("corrupted command");
5366	break;
5367	}
5368
5369	err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data);
5370	if (err) {
5371	struct l2cap_cmd_rej_unk rej;
5372
5373	BT_ERR("Wrong link type (%d)", err);
5374
5375	rej.reason = __constant_cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
5376	l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ,
5377	sizeof(rej), &rej);
5378	}
5379
5380	data += cmd_len;
5381	len -= cmd_len;
5382	}
5383
5384	drop:
5385	kfree_skb(skb);
5386	}
5387
5388	static int l2cap_check_fcs(struct l2cap_chan chan, struct sk_buff skb)
5389	{
5390	u16 our_fcs, rcv_fcs;
5391	int hdr_size;
5392
5393	if (test_bit(FLAG_EXT_CTRL, &chan->flags))
5394	hdr_size = L2CAP_EXT_HDR_SIZE;
5395	else
5396	hdr_size = L2CAP_ENH_HDR_SIZE;
5397
5398	if (chan->fcs == L2CAP_FCS_CRC16) {
5399	skb_trim(skb, skb->len - L2CAP_FCS_SIZE);
5400	rcv_fcs = get_unaligned_le16(skb->data + skb->len);
5401	our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
5402
5403	if (our_fcs != rcv_fcs)
5404	return -EBADMSG;
5405	}
5406	return 0;
5407	}
5408
5409	static void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan)
5410	{
5411	struct l2cap_ctrl control;
5412
5413	BT_DBG("chan %p", chan);
5414
5415	memset(&control, 0, sizeof(control));
5416	control.sframe = 1;
5417	control.final = 1;
5418	control.reqseq = chan->buffer_seq;
5419	set_bit(CONN_SEND_FBIT, &chan->conn_state);
5420
5421	if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
5422	control.super = L2CAP_SUPER_RNR;
5423	l2cap_send_sframe(chan, &control);
5424	}
5425
5426	if (test_and_clear_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
5427	chan->unacked_frames > 0)
5428	__set_retrans_timer(chan);
5429
5430	/* Send pending iframes */
5431	l2cap_ertm_send(chan);
5432
5433	if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state) &&
5434	test_bit(CONN_SEND_FBIT, &chan->conn_state)) {
5435	/* F-bit wasn't sent in an s-frame or i-frame yet, so
5436	* send it now.
5437	*/
5438	control.super = L2CAP_SUPER_RR;
5439	l2cap_send_sframe(chan, &control);
5440	}
5441	}
5442
5443	static void append_skb_frag(struct sk_buff skb, struct sk_buff new_frag,
5444	struct sk_buff **last_frag)
5445	{
5446	/* skb->len reflects data in skb as well as all fragments
5447	* skb->data_len reflects only data in fragments
5448	*/
5449	if (!skb_has_frag_list(skb))
5450	skb_shinfo(skb)->frag_list = new_frag;
5451
5452	new_frag->next = NULL;
5453
5454	(*last_frag)->next = new_frag;
5455	*last_frag = new_frag;
5456
5457	skb->len += new_frag->len;
5458	skb->data_len += new_frag->len;
5459	skb->truesize += new_frag->truesize;
5460	}
5461
5462	static int l2cap_reassemble_sdu(struct l2cap_chan chan, struct sk_buff skb,
5463	struct l2cap_ctrl *control)
5464	{
5465	int err = -EINVAL;
5466
5467	switch (control->sar) {
5468	case L2CAP_SAR_UNSEGMENTED:
5469	if (chan->sdu)
5470	break;
5471
5472	err = chan->ops->recv(chan, skb);
5473	break;
5474
5475	case L2CAP_SAR_START:
5476	if (chan->sdu)
5477	break;
5478
5479	chan->sdu_len = get_unaligned_le16(skb->data);
5480	skb_pull(skb, L2CAP_SDULEN_SIZE);
5481
5482	if (chan->sdu_len > chan->imtu) {
5483	err = -EMSGSIZE;
5484	break;
5485	}
5486
5487	if (skb->len >= chan->sdu_len)
5488	break;
5489
5490	chan->sdu = skb;
5491	chan->sdu_last_frag = skb;
5492
5493	skb = NULL;
5494	err = 0;
5495	break;
5496
5497	case L2CAP_SAR_CONTINUE:
5498	if (!chan->sdu)
5499	break;
5500
5501	append_skb_frag(chan->sdu, skb,
5502	&chan->sdu_last_frag);
5503	skb = NULL;
5504
5505	if (chan->sdu->len >= chan->sdu_len)
5506	break;
5507
5508	err = 0;
5509	break;
5510
5511	case L2CAP_SAR_END:
5512	if (!chan->sdu)
5513	break;
5514
5515	append_skb_frag(chan->sdu, skb,
5516	&chan->sdu_last_frag);
5517	skb = NULL;
5518
5519	if (chan->sdu->len != chan->sdu_len)
5520	break;
5521
5522	err = chan->ops->recv(chan, chan->sdu);
5523
5524	if (!err) {
5525	/* Reassembly complete */
5526	chan->sdu = NULL;
5527	chan->sdu_last_frag = NULL;
5528	chan->sdu_len = 0;
5529	}
5530	break;
5531	}
5532
5533	if (err) {
5534	kfree_skb(skb);
5535	kfree_skb(chan->sdu);
5536	chan->sdu = NULL;
5537	chan->sdu_last_frag = NULL;
5538	chan->sdu_len = 0;
5539	}
5540
5541	return err;
5542	}
5543
5544	static int l2cap_resegment(struct l2cap_chan *chan)
5545	{
5546	/* Placeholder */
5547	return 0;
5548	}
5549
5550	void l2cap_chan_busy(struct l2cap_chan *chan, int busy)
5551	{
5552	u8 event;
5553
5554	if (chan->mode != L2CAP_MODE_ERTM)
5555	return;
5556
5557	event = busy ? L2CAP_EV_LOCAL_BUSY_DETECTED : L2CAP_EV_LOCAL_BUSY_CLEAR;
5558	l2cap_tx(chan, NULL, NULL, event);
5559	}
5560
5561	static int l2cap_rx_queued_iframes(struct l2cap_chan *chan)
5562	{
5563	int err = 0;
5564	/* Pass sequential frames to l2cap_reassemble_sdu()
5565	* until a gap is encountered.
5566	*/
5567
5568	BT_DBG("chan %p", chan);
5569
5570	while (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
5571	struct sk_buff *skb;
5572	BT_DBG("Searching for skb with txseq %d (queue len %d)",
5573	chan->buffer_seq, skb_queue_len(&chan->srej_q));
5574
5575	skb = l2cap_ertm_seq_in_queue(&chan->srej_q, chan->buffer_seq);
5576
5577	if (!skb)
5578	break;
5579
5580	skb_unlink(skb, &chan->srej_q);
5581	chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
5582	err = l2cap_reassemble_sdu(chan, skb, &bt_cb(skb)->control);
5583	if (err)
5584	break;
5585	}
5586
5587	if (skb_queue_empty(&chan->srej_q)) {
5588	chan->rx_state = L2CAP_RX_STATE_RECV;
5589	l2cap_send_ack(chan);
5590	}
5591
5592	return err;
5593	}
5594
5595	static void l2cap_handle_srej(struct l2cap_chan *chan,
5596	struct l2cap_ctrl *control)
5597	{
5598	struct sk_buff *skb;
5599
5600	BT_DBG("chan %p, control %p", chan, control);
5601
5602	if (control->reqseq == chan->next_tx_seq) {
5603	BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq);
5604	l2cap_send_disconn_req(chan, ECONNRESET);
5605	return;
5606	}
5607
5608	skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq);
5609
5610	if (skb == NULL) {
5611	BT_DBG("Seq %d not available for retransmission",
5612	control->reqseq);
5613	return;
5614	}
5615
5616	if (chan->max_tx != 0 && bt_cb(skb)->control.retries >= chan->max_tx) {
5617	BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
5618	l2cap_send_disconn_req(chan, ECONNRESET);
5619	return;
5620	}
5621
5622	clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
5623
5624	if (control->poll) {
5625	l2cap_pass_to_tx(chan, control);
5626
5627	set_bit(CONN_SEND_FBIT, &chan->conn_state);
5628	l2cap_retransmit(chan, control);
5629	l2cap_ertm_send(chan);
5630
5631	if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) {
5632	set_bit(CONN_SREJ_ACT, &chan->conn_state);
5633	chan->srej_save_reqseq = control->reqseq;
5634	}
5635	} else {
5636	l2cap_pass_to_tx_fbit(chan, control);
5637
5638	if (control->final) {
5639	if (chan->srej_save_reqseq != control->reqseq \|\|
5640	!test_and_clear_bit(CONN_SREJ_ACT,
5641	&chan->conn_state))
5642	l2cap_retransmit(chan, control);
5643	} else {
5644	l2cap_retransmit(chan, control);
5645	if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) {
5646	set_bit(CONN_SREJ_ACT, &chan->conn_state);
5647	chan->srej_save_reqseq = control->reqseq;
5648	}
5649	}
5650	}
5651	}
5652
5653	static void l2cap_handle_rej(struct l2cap_chan *chan,
5654	struct l2cap_ctrl *control)
5655	{
5656	struct sk_buff *skb;
5657
5658	BT_DBG("chan %p, control %p", chan, control);
5659
5660	if (control->reqseq == chan->next_tx_seq) {
5661	BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq);
5662	l2cap_send_disconn_req(chan, ECONNRESET);
5663	return;
5664	}
5665
5666	skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq);
5667
5668	if (chan->max_tx && skb &&
5669	bt_cb(skb)->control.retries >= chan->max_tx) {
5670	BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
5671	l2cap_send_disconn_req(chan, ECONNRESET);
5672	return;
5673	}
5674
5675	clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
5676
5677	l2cap_pass_to_tx(chan, control);
5678
5679	if (control->final) {
5680	if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
5681	l2cap_retransmit_all(chan, control);
5682	} else {
5683	l2cap_retransmit_all(chan, control);
5684	l2cap_ertm_send(chan);
5685	if (chan->tx_state == L2CAP_TX_STATE_WAIT_F)
5686	set_bit(CONN_REJ_ACT, &chan->conn_state);
5687	}
5688	}
5689
5690	static u8 l2cap_classify_txseq(struct l2cap_chan *chan, u16 txseq)
5691	{
5692	BT_DBG("chan %p, txseq %d", chan, txseq);
5693
5694	BT_DBG("last_acked_seq %d, expected_tx_seq %d", chan->last_acked_seq,
5695	chan->expected_tx_seq);
5696
5697	if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
5698	if (__seq_offset(chan, txseq, chan->last_acked_seq) >=
5699	chan->tx_win) {
5700	/* See notes below regarding "double poll" and
5701	* invalid packets.
5702	*/
5703	if (chan->tx_win <= ((chan->tx_win_max + 1) >> 1)) {
5704	BT_DBG("Invalid/Ignore - after SREJ");
5705	return L2CAP_TXSEQ_INVALID_IGNORE;
5706	} else {
5707	BT_DBG("Invalid - in window after SREJ sent");
5708	return L2CAP_TXSEQ_INVALID;
5709	}
5710	}
5711
5712	if (chan->srej_list.head == txseq) {
5713	BT_DBG("Expected SREJ");
5714	return L2CAP_TXSEQ_EXPECTED_SREJ;
5715	}
5716
5717	if (l2cap_ertm_seq_in_queue(&chan->srej_q, txseq)) {
5718	BT_DBG("Duplicate SREJ - txseq already stored");
5719	return L2CAP_TXSEQ_DUPLICATE_SREJ;
5720	}
5721
5722	if (l2cap_seq_list_contains(&chan->srej_list, txseq)) {
5723	BT_DBG("Unexpected SREJ - not requested");
5724	return L2CAP_TXSEQ_UNEXPECTED_SREJ;
5725	}
5726	}
5727
5728	if (chan->expected_tx_seq == txseq) {
5729	if (__seq_offset(chan, txseq, chan->last_acked_seq) >=
5730	chan->tx_win) {
5731	BT_DBG("Invalid - txseq outside tx window");
5732	return L2CAP_TXSEQ_INVALID;
5733	} else {
5734	BT_DBG("Expected");
5735	return L2CAP_TXSEQ_EXPECTED;
5736	}
5737	}
5738
5739	if (__seq_offset(chan, txseq, chan->last_acked_seq) <
5740	__seq_offset(chan, chan->expected_tx_seq, chan->last_acked_seq)) {
5741	BT_DBG("Duplicate - expected_tx_seq later than txseq");
5742	return L2CAP_TXSEQ_DUPLICATE;
5743	}
5744
5745	if (__seq_offset(chan, txseq, chan->last_acked_seq) >= chan->tx_win) {
5746	/* A source of invalid packets is a "double poll" condition,
5747	* where delays cause us to send multiple poll packets. If
5748	* the remote stack receives and processes both polls,
5749	* sequence numbers can wrap around in such a way that a
5750	* resent frame has a sequence number that looks like new data
5751	* with a sequence gap. This would trigger an erroneous SREJ
5752	* request.
5753	*
5754	* Fortunately, this is impossible with a tx window that's
5755	* less than half of the maximum sequence number, which allows
5756	* invalid frames to be safely ignored.
5757	*
5758	* With tx window sizes greater than half of the tx window
5759	* maximum, the frame is invalid and cannot be ignored. This
5760	* causes a disconnect.
5761	*/
5762
5763	if (chan->tx_win <= ((chan->tx_win_max + 1) >> 1)) {
5764	BT_DBG("Invalid/Ignore - txseq outside tx window");
5765	return L2CAP_TXSEQ_INVALID_IGNORE;
5766	} else {
5767	BT_DBG("Invalid - txseq outside tx window");
5768	return L2CAP_TXSEQ_INVALID;
5769	}
5770	} else {
5771	BT_DBG("Unexpected - txseq indicates missing frames");
5772	return L2CAP_TXSEQ_UNEXPECTED;
5773	}
5774	}
5775
5776	static int l2cap_rx_state_recv(struct l2cap_chan *chan,
5777	struct l2cap_ctrl *control,
5778	struct sk_buff *skb, u8 event)
5779	{
5780	int err = 0;
5781	bool skb_in_use = false;
5782
5783	BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
5784	event);
5785
5786	switch (event) {
5787	case L2CAP_EV_RECV_IFRAME:
5788	switch (l2cap_classify_txseq(chan, control->txseq)) {
5789	case L2CAP_TXSEQ_EXPECTED:
5790	l2cap_pass_to_tx(chan, control);
5791
5792	if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
5793	BT_DBG("Busy, discarding expected seq %d",
5794	control->txseq);
5795	break;
5796	}
5797
5798	chan->expected_tx_seq = __next_seq(chan,
5799	control->txseq);
5800
5801	chan->buffer_seq = chan->expected_tx_seq;
5802	skb_in_use = true;
5803
5804	err = l2cap_reassemble_sdu(chan, skb, control);
5805	if (err)
5806	break;
5807
5808	if (control->final) {
5809	if (!test_and_clear_bit(CONN_REJ_ACT,
5810	&chan->conn_state)) {
5811	control->final = 0;
5812	l2cap_retransmit_all(chan, control);
5813	l2cap_ertm_send(chan);
5814	}
5815	}
5816
5817	if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state))