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

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

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