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

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

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