~ [ source navigation ] ~ [ diff markup ] ~ [ identifier search ] ~

TOMOYO Linux Cross Reference
Linux/net/bluetooth/l2cap_core.c

Version: ~ [ linux-5.4-rc7 ] ~ [ linux-5.3.11 ] ~ [ linux-5.2.21 ] ~ [ linux-5.1.21 ] ~ [ linux-5.0.21 ] ~ [ linux-4.20.17 ] ~ [ linux-4.19.84 ] ~ [ linux-4.18.20 ] ~ [ linux-4.17.19 ] ~ [ linux-4.16.18 ] ~ [ linux-4.15.18 ] ~ [ linux-4.14.154 ] ~ [ linux-4.13.16 ] ~ [ linux-4.12.14 ] ~ [ linux-4.11.12 ] ~ [ linux-4.10.17 ] ~ [ linux-4.9.201 ] ~ [ linux-4.8.17 ] ~ [ linux-4.7.10 ] ~ [ linux-4.6.7 ] ~ [ linux-4.5.7 ] ~ [ linux-4.4.201 ] ~ [ linux-4.3.6 ] ~ [ linux-4.2.8 ] ~ [ linux-4.1.52 ] ~ [ linux-4.0.9 ] ~ [ linux-3.19.8 ] ~ [ linux-3.18.140 ] ~ [ linux-3.17.8 ] ~ [ linux-3.16.77 ] ~ [ linux-3.15.10 ] ~ [ linux-3.14.79 ] ~ [ linux-3.13.11 ] ~ [ linux-3.12.74 ] ~ [ linux-3.11.10 ] ~ [ linux-3.10.108 ] ~ [ linux-3.9.11 ] ~ [ linux-3.8.13 ] ~ [ linux-3.7.10 ] ~ [ linux-3.6.11 ] ~ [ linux-3.5.7 ] ~ [ linux-3.4.113 ] ~ [ linux-3.3.8 ] ~ [ linux-3.2.102 ] ~ [ linux-3.1.10 ] ~ [ linux-3.0.101 ] ~ [ linux-2.6.32.71 ] ~ [ linux-2.6.0 ] ~ [ linux-2.4.37.11 ] ~ [ unix-v6-master ] ~ [ ccs-tools-1.8.5 ] ~ [ policy-sample ] ~
Architecture: ~ [ i386 ] ~ [ alpha ] ~ [ m68k ] ~ [ mips ] ~ [ ppc ] ~ [ sparc ] ~ [ sparc64 ] ~

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