Bluetooth: invert locking order in connect path
[linux-2.6.git] / net / bluetooth / l2cap_core.c
1 /*
2    BlueZ - Bluetooth protocol stack for Linux
3    Copyright (C) 2000-2001 Qualcomm Incorporated
4    Copyright (C) 2009-2010 Gustavo F. Padovan <gustavo@padovan.org>
5    Copyright (C) 2010 Google Inc.
6
7    Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.com>
8
9    This program is free software; you can redistribute it and/or modify
10    it under the terms of the GNU General Public License version 2 as
11    published by the Free Software Foundation;
12
13    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
14    OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS.
16    IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY
17    CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES
18    WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
19    ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
20    OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
21
22    ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS,
23    COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS
24    SOFTWARE IS DISCLAIMED.
25 */
26
27 /* Bluetooth L2CAP core. */
28
29 #include <linux/module.h>
30
31 #include <linux/types.h>
32 #include <linux/capability.h>
33 #include <linux/errno.h>
34 #include <linux/kernel.h>
35 #include <linux/sched.h>
36 #include <linux/slab.h>
37 #include <linux/poll.h>
38 #include <linux/fcntl.h>
39 #include <linux/init.h>
40 #include <linux/interrupt.h>
41 #include <linux/socket.h>
42 #include <linux/skbuff.h>
43 #include <linux/list.h>
44 #include <linux/device.h>
45 #include <linux/debugfs.h>
46 #include <linux/seq_file.h>
47 #include <linux/uaccess.h>
48 #include <linux/crc16.h>
49 #include <net/sock.h>
50
51 #include <asm/system.h>
52 #include <asm/unaligned.h>
53
54 #include <net/bluetooth/bluetooth.h>
55 #include <net/bluetooth/hci_core.h>
56 #include <net/bluetooth/l2cap.h>
57 #include <net/bluetooth/smp.h>
58
59 int disable_ertm;
60
61 static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN;
62 static u8 l2cap_fixed_chan[8] = { L2CAP_FC_L2CAP, };
63
64 static LIST_HEAD(chan_list);
65 static DEFINE_RWLOCK(chan_list_lock);
66
67 static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
68                                 u8 code, u8 ident, u16 dlen, void *data);
69 static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len,
70                                                                 void *data);
71 static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data);
72 static void l2cap_send_disconn_req(struct l2cap_conn *conn,
73                                 struct l2cap_chan *chan, int err);
74
75 static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb);
76
77 /* ---- L2CAP channels ---- */
78
79 static inline void chan_hold(struct l2cap_chan *c)
80 {
81         atomic_inc(&c->refcnt);
82 }
83
84 static inline void chan_put(struct l2cap_chan *c)
85 {
86         if (atomic_dec_and_test(&c->refcnt))
87                 kfree(c);
88 }
89
90 static struct l2cap_chan *__l2cap_get_chan_by_dcid(struct l2cap_conn *conn, u16 cid)
91 {
92         struct l2cap_chan *c;
93
94         list_for_each_entry(c, &conn->chan_l, list) {
95                 if (c->dcid == cid)
96                         return c;
97         }
98         return NULL;
99 }
100
101 static struct l2cap_chan *__l2cap_get_chan_by_scid(struct l2cap_conn *conn, u16 cid)
102 {
103         struct l2cap_chan *c;
104
105         list_for_each_entry(c, &conn->chan_l, list) {
106                 if (c->scid == cid)
107                         return c;
108         }
109         return NULL;
110 }
111
112 /* Find channel with given SCID.
113  * Returns locked socket */
114 static struct l2cap_chan *l2cap_get_chan_by_scid(struct l2cap_conn *conn, u16 cid)
115 {
116         struct l2cap_chan *c;
117
118         mutex_lock(&conn->chan_lock);
119         c = __l2cap_get_chan_by_scid(conn, cid);
120         if (c)
121                 lock_sock(c->sk);
122         mutex_unlock(&conn->chan_lock);
123         return c;
124 }
125
126 static struct l2cap_chan *__l2cap_get_chan_by_ident(struct l2cap_conn *conn, u8 ident)
127 {
128         struct l2cap_chan *c;
129
130         list_for_each_entry(c, &conn->chan_l, list) {
131                 if (c->ident == ident)
132                         return c;
133         }
134         return NULL;
135 }
136
137 static inline struct l2cap_chan *l2cap_get_chan_by_ident(struct l2cap_conn *conn, u8 ident)
138 {
139         struct l2cap_chan *c;
140
141         mutex_lock(&conn->chan_lock);
142         c = __l2cap_get_chan_by_ident(conn, ident);
143         if (c)
144                 lock_sock(c->sk);
145         mutex_unlock(&conn->chan_lock);
146         return c;
147 }
148
149 static struct l2cap_chan *__l2cap_global_chan_by_addr(__le16 psm, bdaddr_t *src)
150 {
151         struct l2cap_chan *c;
152
153         list_for_each_entry(c, &chan_list, global_l) {
154                 if (c->sport == psm && !bacmp(&bt_sk(c->sk)->src, src))
155                         return c;
156         }
157         return NULL;
158 }
159
160 int l2cap_add_psm(struct l2cap_chan *chan, bdaddr_t *src, __le16 psm)
161 {
162         int err;
163
164         write_lock_bh(&chan_list_lock);
165
166         if (psm && __l2cap_global_chan_by_addr(psm, src)) {
167                 err = -EADDRINUSE;
168                 goto done;
169         }
170
171         if (psm) {
172                 chan->psm = psm;
173                 chan->sport = psm;
174                 err = 0;
175         } else {
176                 u16 p;
177
178                 err = -EINVAL;
179                 for (p = 0x1001; p < 0x1100; p += 2)
180                         if (!__l2cap_global_chan_by_addr(cpu_to_le16(p), src)) {
181                                 chan->psm   = cpu_to_le16(p);
182                                 chan->sport = cpu_to_le16(p);
183                                 err = 0;
184                                 break;
185                         }
186         }
187
188 done:
189         write_unlock_bh(&chan_list_lock);
190         return err;
191 }
192
193 int l2cap_add_scid(struct l2cap_chan *chan,  __u16 scid)
194 {
195         write_lock_bh(&chan_list_lock);
196
197         chan->scid = scid;
198
199         write_unlock_bh(&chan_list_lock);
200
201         return 0;
202 }
203
204 static u16 l2cap_alloc_cid(struct l2cap_conn *conn)
205 {
206         u16 cid = L2CAP_CID_DYN_START;
207
208         for (; cid < L2CAP_CID_DYN_END; cid++) {
209                 if (!__l2cap_get_chan_by_scid(conn, cid))
210                         return cid;
211         }
212
213         return 0;
214 }
215
216 static void l2cap_set_timer(struct l2cap_chan *chan, struct delayed_work *work, long timeout)
217 {
218         BT_DBG("chan %p state %d timeout %ld", chan, chan->state, timeout);
219
220         cancel_delayed_work_sync(work);
221
222         schedule_delayed_work(work, timeout);
223 }
224
225 static void l2cap_clear_timer(struct delayed_work *work)
226 {
227         cancel_delayed_work_sync(work);
228 }
229
230 static char *state_to_string(int state)
231 {
232         switch(state) {
233         case BT_CONNECTED:
234                 return "BT_CONNECTED";
235         case BT_OPEN:
236                 return "BT_OPEN";
237         case BT_BOUND:
238                 return "BT_BOUND";
239         case BT_LISTEN:
240                 return "BT_LISTEN";
241         case BT_CONNECT:
242                 return "BT_CONNECT";
243         case BT_CONNECT2:
244                 return "BT_CONNECT2";
245         case BT_CONFIG:
246                 return "BT_CONFIG";
247         case BT_DISCONN:
248                 return "BT_DISCONN";
249         case BT_CLOSED:
250                 return "BT_CLOSED";
251         }
252
253         return "invalid state";
254 }
255
256 static void l2cap_state_change(struct l2cap_chan *chan, int state)
257 {
258         BT_DBG("%p %s -> %s", chan, state_to_string(chan->state),
259                                                 state_to_string(state));
260
261         chan->state = state;
262         chan->ops->state_change(chan->data, state);
263 }
264
265 static void l2cap_chan_timeout(struct work_struct *work)
266 {
267         struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
268                                                         chan_timer.work);
269         struct sock *sk = chan->sk;
270         int reason;
271
272         BT_DBG("chan %p state %d", chan, chan->state);
273
274         lock_sock(sk);
275
276         if (chan->state == BT_CONNECTED || chan->state == BT_CONFIG)
277                 reason = ECONNREFUSED;
278         else if (chan->state == BT_CONNECT &&
279                                         chan->sec_level != BT_SECURITY_SDP)
280                 reason = ECONNREFUSED;
281         else
282                 reason = ETIMEDOUT;
283
284         l2cap_chan_close(chan, reason);
285
286         release_sock(sk);
287
288         chan->ops->close(chan->data);
289         chan_put(chan);
290 }
291
292 struct l2cap_chan *l2cap_chan_create(struct sock *sk)
293 {
294         struct l2cap_chan *chan;
295
296         chan = kzalloc(sizeof(*chan), GFP_ATOMIC);
297         if (!chan)
298                 return NULL;
299
300         chan->sk = sk;
301
302         write_lock_bh(&chan_list_lock);
303         list_add(&chan->global_l, &chan_list);
304         write_unlock_bh(&chan_list_lock);
305
306         INIT_DELAYED_WORK(&chan->chan_timer, l2cap_chan_timeout);
307
308         chan->state = BT_OPEN;
309
310         atomic_set(&chan->refcnt, 1);
311
312         BT_DBG("sk %p chan %p", sk, chan);
313
314         return chan;
315 }
316
317 void l2cap_chan_destroy(struct l2cap_chan *chan)
318 {
319         write_lock_bh(&chan_list_lock);
320         list_del(&chan->global_l);
321         write_unlock_bh(&chan_list_lock);
322
323         chan_put(chan);
324 }
325
326 static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
327 {
328         BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn,
329                         chan->psm, chan->dcid);
330
331         conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM;
332
333         chan->conn = conn;
334
335         if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED) {
336                 if (conn->hcon->type == LE_LINK) {
337                         /* LE connection */
338                         chan->omtu = L2CAP_LE_DEFAULT_MTU;
339                         chan->scid = L2CAP_CID_LE_DATA;
340                         chan->dcid = L2CAP_CID_LE_DATA;
341                 } else {
342                         /* Alloc CID for connection-oriented socket */
343                         chan->scid = l2cap_alloc_cid(conn);
344                         chan->omtu = L2CAP_DEFAULT_MTU;
345                 }
346         } else if (chan->chan_type == L2CAP_CHAN_CONN_LESS) {
347                 /* Connectionless socket */
348                 chan->scid = L2CAP_CID_CONN_LESS;
349                 chan->dcid = L2CAP_CID_CONN_LESS;
350                 chan->omtu = L2CAP_DEFAULT_MTU;
351         } else {
352                 /* Raw socket can send/recv signalling messages only */
353                 chan->scid = L2CAP_CID_SIGNALING;
354                 chan->dcid = L2CAP_CID_SIGNALING;
355                 chan->omtu = L2CAP_DEFAULT_MTU;
356         }
357
358         chan->local_id          = L2CAP_BESTEFFORT_ID;
359         chan->local_stype       = L2CAP_SERV_BESTEFFORT;
360         chan->local_msdu        = L2CAP_DEFAULT_MAX_SDU_SIZE;
361         chan->local_sdu_itime   = L2CAP_DEFAULT_SDU_ITIME;
362         chan->local_acc_lat     = L2CAP_DEFAULT_ACC_LAT;
363         chan->local_flush_to    = L2CAP_DEFAULT_FLUSH_TO;
364
365         chan_hold(chan);
366
367         list_add(&chan->list, &conn->chan_l);
368 }
369
370 /* Delete channel.
371  * Must be called on the locked socket. */
372 static void l2cap_chan_del(struct l2cap_chan *chan, int err)
373 {
374         struct sock *sk = chan->sk;
375         struct l2cap_conn *conn = chan->conn;
376         struct sock *parent = bt_sk(sk)->parent;
377
378         __clear_chan_timer(chan);
379
380         BT_DBG("chan %p, conn %p, err %d", chan, conn, err);
381
382         if (conn) {
383                 /* Delete from channel list */
384                 mutex_lock(&conn->chan_lock);
385                 list_del(&chan->list);
386                 mutex_unlock(&conn->chan_lock);
387                 chan_put(chan);
388
389                 chan->conn = NULL;
390                 hci_conn_put(conn->hcon);
391         }
392
393         l2cap_state_change(chan, BT_CLOSED);
394         sock_set_flag(sk, SOCK_ZAPPED);
395
396         if (err)
397                 sk->sk_err = err;
398
399         if (parent) {
400                 bt_accept_unlink(sk);
401                 parent->sk_data_ready(parent, 0);
402         } else
403                 sk->sk_state_change(sk);
404
405         if (!(test_bit(CONF_OUTPUT_DONE, &chan->conf_state) &&
406                         test_bit(CONF_INPUT_DONE, &chan->conf_state)))
407                 return;
408
409         skb_queue_purge(&chan->tx_q);
410
411         if (chan->mode == L2CAP_MODE_ERTM) {
412                 struct srej_list *l, *tmp;
413
414                 __clear_retrans_timer(chan);
415                 __clear_monitor_timer(chan);
416                 __clear_ack_timer(chan);
417
418                 skb_queue_purge(&chan->srej_q);
419
420                 list_for_each_entry_safe(l, tmp, &chan->srej_l, list) {
421                         list_del(&l->list);
422                         kfree(l);
423                 }
424         }
425 }
426
427 static void l2cap_chan_cleanup_listen(struct sock *parent)
428 {
429         struct sock *sk;
430
431         BT_DBG("parent %p", parent);
432
433         /* Close not yet accepted channels */
434         while ((sk = bt_accept_dequeue(parent, NULL))) {
435                 struct l2cap_chan *chan = l2cap_pi(sk)->chan;
436                 __clear_chan_timer(chan);
437                 lock_sock(sk);
438                 l2cap_chan_close(chan, ECONNRESET);
439                 release_sock(sk);
440                 chan->ops->close(chan->data);
441         }
442 }
443
444 void l2cap_chan_close(struct l2cap_chan *chan, int reason)
445 {
446         struct l2cap_conn *conn = chan->conn;
447         struct sock *sk = chan->sk;
448
449         BT_DBG("chan %p state %d socket %p", chan, chan->state, sk->sk_socket);
450
451         switch (chan->state) {
452         case BT_LISTEN:
453                 l2cap_chan_cleanup_listen(sk);
454
455                 l2cap_state_change(chan, BT_CLOSED);
456                 sock_set_flag(sk, SOCK_ZAPPED);
457                 break;
458
459         case BT_CONNECTED:
460         case BT_CONFIG:
461                 if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED &&
462                                         conn->hcon->type == ACL_LINK) {
463                         __clear_chan_timer(chan);
464                         __set_chan_timer(chan, sk->sk_sndtimeo);
465                         l2cap_send_disconn_req(conn, chan, reason);
466                 } else
467                         l2cap_chan_del(chan, reason);
468                 break;
469
470         case BT_CONNECT2:
471                 if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED &&
472                                         conn->hcon->type == ACL_LINK) {
473                         struct l2cap_conn_rsp rsp;
474                         __u16 result;
475
476                         if (bt_sk(sk)->defer_setup)
477                                 result = L2CAP_CR_SEC_BLOCK;
478                         else
479                                 result = L2CAP_CR_BAD_PSM;
480                         l2cap_state_change(chan, BT_DISCONN);
481
482                         rsp.scid   = cpu_to_le16(chan->dcid);
483                         rsp.dcid   = cpu_to_le16(chan->scid);
484                         rsp.result = cpu_to_le16(result);
485                         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
486                         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
487                                                         sizeof(rsp), &rsp);
488                 }
489
490                 l2cap_chan_del(chan, reason);
491                 break;
492
493         case BT_CONNECT:
494         case BT_DISCONN:
495                 l2cap_chan_del(chan, reason);
496                 break;
497
498         default:
499                 sock_set_flag(sk, SOCK_ZAPPED);
500                 break;
501         }
502 }
503
504 static inline u8 l2cap_get_auth_type(struct l2cap_chan *chan)
505 {
506         if (chan->chan_type == L2CAP_CHAN_RAW) {
507                 switch (chan->sec_level) {
508                 case BT_SECURITY_HIGH:
509                         return HCI_AT_DEDICATED_BONDING_MITM;
510                 case BT_SECURITY_MEDIUM:
511                         return HCI_AT_DEDICATED_BONDING;
512                 default:
513                         return HCI_AT_NO_BONDING;
514                 }
515         } else if (chan->psm == cpu_to_le16(0x0001)) {
516                 if (chan->sec_level == BT_SECURITY_LOW)
517                         chan->sec_level = BT_SECURITY_SDP;
518
519                 if (chan->sec_level == BT_SECURITY_HIGH)
520                         return HCI_AT_NO_BONDING_MITM;
521                 else
522                         return HCI_AT_NO_BONDING;
523         } else {
524                 switch (chan->sec_level) {
525                 case BT_SECURITY_HIGH:
526                         return HCI_AT_GENERAL_BONDING_MITM;
527                 case BT_SECURITY_MEDIUM:
528                         return HCI_AT_GENERAL_BONDING;
529                 default:
530                         return HCI_AT_NO_BONDING;
531                 }
532         }
533 }
534
535 /* Service level security */
536 int l2cap_chan_check_security(struct l2cap_chan *chan)
537 {
538         struct l2cap_conn *conn = chan->conn;
539         __u8 auth_type;
540
541         auth_type = l2cap_get_auth_type(chan);
542
543         return hci_conn_security(conn->hcon, chan->sec_level, auth_type);
544 }
545
546 static u8 l2cap_get_ident(struct l2cap_conn *conn)
547 {
548         u8 id;
549
550         /* Get next available identificator.
551          *    1 - 128 are used by kernel.
552          *  129 - 199 are reserved.
553          *  200 - 254 are used by utilities like l2ping, etc.
554          */
555
556         spin_lock_bh(&conn->lock);
557
558         if (++conn->tx_ident > 128)
559                 conn->tx_ident = 1;
560
561         id = conn->tx_ident;
562
563         spin_unlock_bh(&conn->lock);
564
565         return id;
566 }
567
568 static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, void *data)
569 {
570         struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data);
571         u8 flags;
572
573         BT_DBG("code 0x%2.2x", code);
574
575         if (!skb)
576                 return;
577
578         if (lmp_no_flush_capable(conn->hcon->hdev))
579                 flags = ACL_START_NO_FLUSH;
580         else
581                 flags = ACL_START;
582
583         bt_cb(skb)->force_active = BT_POWER_FORCE_ACTIVE_ON;
584         skb->priority = HCI_PRIO_MAX;
585
586         hci_send_acl(conn->hchan, skb, flags);
587 }
588
589 static void l2cap_do_send(struct l2cap_chan *chan, struct sk_buff *skb)
590 {
591         struct hci_conn *hcon = chan->conn->hcon;
592         u16 flags;
593
594         BT_DBG("chan %p, skb %p len %d priority %u", chan, skb, skb->len,
595                                                         skb->priority);
596
597         if (!test_bit(FLAG_FLUSHABLE, &chan->flags) &&
598                                         lmp_no_flush_capable(hcon->hdev))
599                 flags = ACL_START_NO_FLUSH;
600         else
601                 flags = ACL_START;
602
603         bt_cb(skb)->force_active = test_bit(FLAG_FORCE_ACTIVE, &chan->flags);
604         hci_send_acl(chan->conn->hchan, skb, flags);
605 }
606
607 static inline void l2cap_send_sframe(struct l2cap_chan *chan, u32 control)
608 {
609         struct sk_buff *skb;
610         struct l2cap_hdr *lh;
611         struct l2cap_conn *conn = chan->conn;
612         int count, hlen;
613
614         if (chan->state != BT_CONNECTED)
615                 return;
616
617         if (test_bit(FLAG_EXT_CTRL, &chan->flags))
618                 hlen = L2CAP_EXT_HDR_SIZE;
619         else
620                 hlen = L2CAP_ENH_HDR_SIZE;
621
622         if (chan->fcs == L2CAP_FCS_CRC16)
623                 hlen += L2CAP_FCS_SIZE;
624
625         BT_DBG("chan %p, control 0x%8.8x", chan, control);
626
627         count = min_t(unsigned int, conn->mtu, hlen);
628
629         control |= __set_sframe(chan);
630
631         if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
632                 control |= __set_ctrl_final(chan);
633
634         if (test_and_clear_bit(CONN_SEND_PBIT, &chan->conn_state))
635                 control |= __set_ctrl_poll(chan);
636
637         skb = bt_skb_alloc(count, GFP_ATOMIC);
638         if (!skb)
639                 return;
640
641         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
642         lh->len = cpu_to_le16(hlen - L2CAP_HDR_SIZE);
643         lh->cid = cpu_to_le16(chan->dcid);
644
645         __put_control(chan, control, skb_put(skb, __ctrl_size(chan)));
646
647         if (chan->fcs == L2CAP_FCS_CRC16) {
648                 u16 fcs = crc16(0, (u8 *)lh, count - L2CAP_FCS_SIZE);
649                 put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE));
650         }
651
652         skb->priority = HCI_PRIO_MAX;
653         l2cap_do_send(chan, skb);
654 }
655
656 static inline void l2cap_send_rr_or_rnr(struct l2cap_chan *chan, u32 control)
657 {
658         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
659                 control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR);
660                 set_bit(CONN_RNR_SENT, &chan->conn_state);
661         } else
662                 control |= __set_ctrl_super(chan, L2CAP_SUPER_RR);
663
664         control |= __set_reqseq(chan, chan->buffer_seq);
665
666         l2cap_send_sframe(chan, control);
667 }
668
669 static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan)
670 {
671         return !test_bit(CONF_CONNECT_PEND, &chan->conf_state);
672 }
673
674 static void l2cap_do_start(struct l2cap_chan *chan)
675 {
676         struct l2cap_conn *conn = chan->conn;
677
678         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) {
679                 if (!(conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE))
680                         return;
681
682                 if (l2cap_chan_check_security(chan) &&
683                                 __l2cap_no_conn_pending(chan)) {
684                         struct l2cap_conn_req req;
685                         req.scid = cpu_to_le16(chan->scid);
686                         req.psm  = chan->psm;
687
688                         chan->ident = l2cap_get_ident(conn);
689                         set_bit(CONF_CONNECT_PEND, &chan->conf_state);
690
691                         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_REQ,
692                                                         sizeof(req), &req);
693                 }
694         } else {
695                 struct l2cap_info_req req;
696                 req.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
697
698                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
699                 conn->info_ident = l2cap_get_ident(conn);
700
701                 schedule_delayed_work(&conn->info_work,
702                                         msecs_to_jiffies(L2CAP_INFO_TIMEOUT));
703
704                 l2cap_send_cmd(conn, conn->info_ident,
705                                         L2CAP_INFO_REQ, sizeof(req), &req);
706         }
707 }
708
709 static inline int l2cap_mode_supported(__u8 mode, __u32 feat_mask)
710 {
711         u32 local_feat_mask = l2cap_feat_mask;
712         if (!disable_ertm)
713                 local_feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING;
714
715         switch (mode) {
716         case L2CAP_MODE_ERTM:
717                 return L2CAP_FEAT_ERTM & feat_mask & local_feat_mask;
718         case L2CAP_MODE_STREAMING:
719                 return L2CAP_FEAT_STREAMING & feat_mask & local_feat_mask;
720         default:
721                 return 0x00;
722         }
723 }
724
725 static void l2cap_send_disconn_req(struct l2cap_conn *conn, struct l2cap_chan *chan, int err)
726 {
727         struct sock *sk;
728         struct l2cap_disconn_req req;
729
730         if (!conn)
731                 return;
732
733         sk = chan->sk;
734
735         if (chan->mode == L2CAP_MODE_ERTM) {
736                 __clear_retrans_timer(chan);
737                 __clear_monitor_timer(chan);
738                 __clear_ack_timer(chan);
739         }
740
741         req.dcid = cpu_to_le16(chan->dcid);
742         req.scid = cpu_to_le16(chan->scid);
743         l2cap_send_cmd(conn, l2cap_get_ident(conn),
744                         L2CAP_DISCONN_REQ, sizeof(req), &req);
745
746         l2cap_state_change(chan, BT_DISCONN);
747         sk->sk_err = err;
748 }
749
750 /* ---- L2CAP connections ---- */
751 static void l2cap_conn_start(struct l2cap_conn *conn)
752 {
753         struct l2cap_chan *chan, *tmp;
754
755         BT_DBG("conn %p", conn);
756
757         mutex_lock(&conn->chan_lock);
758
759         list_for_each_entry_safe(chan, tmp, &conn->chan_l, list) {
760                 struct sock *sk = chan->sk;
761
762                 bh_lock_sock(sk);
763
764                 if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
765                         bh_unlock_sock(sk);
766                         continue;
767                 }
768
769                 if (chan->state == BT_CONNECT) {
770                         struct l2cap_conn_req req;
771
772                         if (!l2cap_chan_check_security(chan) ||
773                                         !__l2cap_no_conn_pending(chan)) {
774                                 bh_unlock_sock(sk);
775                                 continue;
776                         }
777
778                         if (!l2cap_mode_supported(chan->mode, conn->feat_mask)
779                                         && test_bit(CONF_STATE2_DEVICE,
780                                         &chan->conf_state)) {
781                                 /* l2cap_chan_close() calls list_del(chan)
782                                  * so release the lock */
783                                 mutex_unlock(&conn->chan_lock);
784                                 l2cap_chan_close(chan, ECONNRESET);
785                                 utex_lock(&conn->chan_lock);
786                                 bh_unlock_sock(sk);
787                                 continue;
788                         }
789
790                         req.scid = cpu_to_le16(chan->scid);
791                         req.psm  = chan->psm;
792
793                         chan->ident = l2cap_get_ident(conn);
794                         set_bit(CONF_CONNECT_PEND, &chan->conf_state);
795
796                         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_REQ,
797                                                         sizeof(req), &req);
798
799                 } else if (chan->state == BT_CONNECT2) {
800                         struct l2cap_conn_rsp rsp;
801                         char buf[128];
802                         rsp.scid = cpu_to_le16(chan->dcid);
803                         rsp.dcid = cpu_to_le16(chan->scid);
804
805                         if (l2cap_chan_check_security(chan)) {
806                                 if (bt_sk(sk)->defer_setup) {
807                                         struct sock *parent = bt_sk(sk)->parent;
808                                         rsp.result = cpu_to_le16(L2CAP_CR_PEND);
809                                         rsp.status = cpu_to_le16(L2CAP_CS_AUTHOR_PEND);
810                                         if (parent)
811                                                 parent->sk_data_ready(parent, 0);
812
813                                 } else {
814                                         l2cap_state_change(chan, BT_CONFIG);
815                                         rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
816                                         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
817                                 }
818                         } else {
819                                 rsp.result = cpu_to_le16(L2CAP_CR_PEND);
820                                 rsp.status = cpu_to_le16(L2CAP_CS_AUTHEN_PEND);
821                         }
822
823                         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
824                                                         sizeof(rsp), &rsp);
825
826                         if (test_bit(CONF_REQ_SENT, &chan->conf_state) ||
827                                         rsp.result != L2CAP_CR_SUCCESS) {
828                                 bh_unlock_sock(sk);
829                                 continue;
830                         }
831
832                         set_bit(CONF_REQ_SENT, &chan->conf_state);
833                         l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
834                                                 l2cap_build_conf_req(chan, buf), buf);
835                         chan->num_conf_req++;
836                 }
837
838                 bh_unlock_sock(sk);
839         }
840
841         mutex_unlock(&conn->chan_lock);
842 }
843
844 /* Find socket with cid and source bdaddr.
845  * Returns closest match, locked.
846  */
847 static struct l2cap_chan *l2cap_global_chan_by_scid(int state, __le16 cid, bdaddr_t *src)
848 {
849         struct l2cap_chan *c, *c1 = NULL;
850
851         read_lock(&chan_list_lock);
852
853         list_for_each_entry(c, &chan_list, global_l) {
854                 struct sock *sk = c->sk;
855
856                 if (state && c->state != state)
857                         continue;
858
859                 if (c->scid == cid) {
860                         /* Exact match. */
861                         if (!bacmp(&bt_sk(sk)->src, src)) {
862                                 read_unlock(&chan_list_lock);
863                                 return c;
864                         }
865
866                         /* Closest match */
867                         if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY))
868                                 c1 = c;
869                 }
870         }
871
872         read_unlock(&chan_list_lock);
873
874         return c1;
875 }
876
877 static void l2cap_le_conn_ready(struct l2cap_conn *conn)
878 {
879         struct sock *parent, *sk;
880         struct l2cap_chan *chan, *pchan;
881
882         BT_DBG("");
883
884         /* Check if we have socket listening on cid */
885         pchan = l2cap_global_chan_by_scid(BT_LISTEN, L2CAP_CID_LE_DATA,
886                                                         conn->src);
887         if (!pchan)
888                 return;
889
890         parent = pchan->sk;
891
892         lock_sock(parent);
893
894         /* Check for backlog size */
895         if (sk_acceptq_is_full(parent)) {
896                 BT_DBG("backlog full %d", parent->sk_ack_backlog);
897                 goto clean;
898         }
899
900         chan = pchan->ops->new_connection(pchan->data);
901         if (!chan)
902                 goto clean;
903
904         sk = chan->sk;
905
906         mutex_lock(&conn->chan_lock);
907
908         hci_conn_hold(conn->hcon);
909
910         bacpy(&bt_sk(sk)->src, conn->src);
911         bacpy(&bt_sk(sk)->dst, conn->dst);
912
913         bt_accept_enqueue(parent, sk);
914
915         __l2cap_chan_add(conn, chan);
916
917         __set_chan_timer(chan, sk->sk_sndtimeo);
918
919         l2cap_state_change(chan, BT_CONNECTED);
920         parent->sk_data_ready(parent, 0);
921
922         mutex_unlock(&conn->chan_lock);
923
924 clean:
925         release_sock(parent);
926 }
927
928 static void l2cap_chan_ready(struct sock *sk)
929 {
930         struct l2cap_chan *chan = l2cap_pi(sk)->chan;
931         struct sock *parent = bt_sk(sk)->parent;
932
933         BT_DBG("sk %p, parent %p", sk, parent);
934
935         chan->conf_state = 0;
936         __clear_chan_timer(chan);
937
938         l2cap_state_change(chan, BT_CONNECTED);
939         sk->sk_state_change(sk);
940
941         if (parent)
942                 parent->sk_data_ready(parent, 0);
943 }
944
945 static void l2cap_conn_ready(struct l2cap_conn *conn)
946 {
947         struct l2cap_chan *chan;
948
949         BT_DBG("conn %p", conn);
950
951         if (!conn->hcon->out && conn->hcon->type == LE_LINK)
952                 l2cap_le_conn_ready(conn);
953
954         if (conn->hcon->out && conn->hcon->type == LE_LINK)
955                 smp_conn_security(conn, conn->hcon->pending_sec_level);
956
957         mutex_lock(&conn->chan_lock);
958
959         list_for_each_entry(chan, &conn->chan_l, list) {
960                 struct sock *sk = chan->sk;
961
962                 bh_lock_sock(sk);
963
964                 if (conn->hcon->type == LE_LINK) {
965                         if (smp_conn_security(conn, chan->sec_level))
966                                 l2cap_chan_ready(sk);
967
968                 } else if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
969                         __clear_chan_timer(chan);
970                         l2cap_state_change(chan, BT_CONNECTED);
971                         sk->sk_state_change(sk);
972
973                 } else if (chan->state == BT_CONNECT)
974                         l2cap_do_start(chan);
975
976                 bh_unlock_sock(sk);
977         }
978
979         mutex_unlock(&conn->chan_lock);
980 }
981
982 /* Notify sockets that we cannot guaranty reliability anymore */
983 static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err)
984 {
985         struct l2cap_chan *chan;
986
987         BT_DBG("conn %p", conn);
988
989         mutex_lock(&conn->chan_lock);
990
991         list_for_each_entry(chan, &conn->chan_l, list) {
992                 struct sock *sk = chan->sk;
993
994                 if (test_bit(FLAG_FORCE_RELIABLE, &chan->flags))
995                         sk->sk_err = err;
996         }
997
998         mutex_unlock(&conn->chan_lock);
999 }
1000
1001 static void l2cap_info_timeout(struct work_struct *work)
1002 {
1003         struct l2cap_conn *conn = container_of(work, struct l2cap_conn,
1004                                                         info_work.work);
1005
1006         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
1007         conn->info_ident = 0;
1008
1009         l2cap_conn_start(conn);
1010 }
1011
1012 static void l2cap_conn_del(struct hci_conn *hcon, int err)
1013 {
1014         struct l2cap_conn *conn = hcon->l2cap_data;
1015         struct l2cap_chan *chan, *l;
1016         struct sock *sk;
1017
1018         if (!conn)
1019                 return;
1020
1021         BT_DBG("hcon %p conn %p, err %d", hcon, conn, err);
1022
1023         kfree_skb(conn->rx_skb);
1024
1025         /* Kill channels */
1026         list_for_each_entry_safe(chan, l, &conn->chan_l, list) {
1027                 sk = chan->sk;
1028                 lock_sock(sk);
1029                 l2cap_chan_del(chan, err);
1030                 release_sock(sk);
1031                 chan->ops->close(chan->data);
1032         }
1033
1034         hci_chan_del(conn->hchan);
1035
1036         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT)
1037                 cancel_delayed_work_sync(&conn->info_work);
1038
1039         if (test_and_clear_bit(HCI_CONN_LE_SMP_PEND, &hcon->pend)) {
1040                 del_timer(&conn->security_timer);
1041                 smp_chan_destroy(conn);
1042         }
1043
1044         hcon->l2cap_data = NULL;
1045         kfree(conn);
1046 }
1047
1048 static void security_timeout(unsigned long arg)
1049 {
1050         struct l2cap_conn *conn = (void *) arg;
1051
1052         l2cap_conn_del(conn->hcon, ETIMEDOUT);
1053 }
1054
1055 static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status)
1056 {
1057         struct l2cap_conn *conn = hcon->l2cap_data;
1058         struct hci_chan *hchan;
1059
1060         if (conn || status)
1061                 return conn;
1062
1063         hchan = hci_chan_create(hcon);
1064         if (!hchan)
1065                 return NULL;
1066
1067         conn = kzalloc(sizeof(struct l2cap_conn), GFP_ATOMIC);
1068         if (!conn) {
1069                 hci_chan_del(hchan);
1070                 return NULL;
1071         }
1072
1073         hcon->l2cap_data = conn;
1074         conn->hcon = hcon;
1075         conn->hchan = hchan;
1076
1077         BT_DBG("hcon %p conn %p hchan %p", hcon, conn, hchan);
1078
1079         if (hcon->hdev->le_mtu && hcon->type == LE_LINK)
1080                 conn->mtu = hcon->hdev->le_mtu;
1081         else
1082                 conn->mtu = hcon->hdev->acl_mtu;
1083
1084         conn->src = &hcon->hdev->bdaddr;
1085         conn->dst = &hcon->dst;
1086
1087         conn->feat_mask = 0;
1088
1089         spin_lock_init(&conn->lock);
1090         mutex_init(&conn->chan_lock);
1091
1092         INIT_LIST_HEAD(&conn->chan_l);
1093
1094         if (hcon->type == LE_LINK)
1095                 setup_timer(&conn->security_timer, security_timeout,
1096                                                 (unsigned long) conn);
1097         else
1098                 INIT_DELAYED_WORK(&conn->info_work, l2cap_info_timeout);
1099
1100         conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM;
1101
1102         return conn;
1103 }
1104
1105 static inline void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
1106 {
1107         mutex_lock(&conn->chan_lock);
1108         __l2cap_chan_add(conn, chan);
1109         mutex_unlock(&conn->chan_lock);
1110 }
1111
1112 /* ---- Socket interface ---- */
1113
1114 /* Find socket with psm and source bdaddr.
1115  * Returns closest match.
1116  */
1117 static struct l2cap_chan *l2cap_global_chan_by_psm(int state, __le16 psm, bdaddr_t *src)
1118 {
1119         struct l2cap_chan *c, *c1 = NULL;
1120
1121         read_lock(&chan_list_lock);
1122
1123         list_for_each_entry(c, &chan_list, global_l) {
1124                 struct sock *sk = c->sk;
1125
1126                 if (state && c->state != state)
1127                         continue;
1128
1129                 if (c->psm == psm) {
1130                         /* Exact match. */
1131                         if (!bacmp(&bt_sk(sk)->src, src)) {
1132                                 read_unlock(&chan_list_lock);
1133                                 return c;
1134                         }
1135
1136                         /* Closest match */
1137                         if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY))
1138                                 c1 = c;
1139                 }
1140         }
1141
1142         read_unlock(&chan_list_lock);
1143
1144         return c1;
1145 }
1146
1147 inline int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid, bdaddr_t *dst)
1148 {
1149         struct sock *sk = chan->sk;
1150         bdaddr_t *src = &bt_sk(sk)->src;
1151         struct l2cap_conn *conn;
1152         struct hci_conn *hcon;
1153         struct hci_dev *hdev;
1154         __u8 auth_type;
1155         int err;
1156
1157         BT_DBG("%s -> %s psm 0x%2.2x", batostr(src), batostr(dst),
1158                                                         chan->psm);
1159
1160         hdev = hci_get_route(dst, src);
1161         if (!hdev)
1162                 return -EHOSTUNREACH;
1163
1164         hci_dev_lock(hdev);
1165
1166         lock_sock(sk);
1167
1168         /* PSM must be odd and lsb of upper byte must be 0 */
1169         if ((__le16_to_cpu(psm) & 0x0101) != 0x0001 && !cid &&
1170                                         chan->chan_type != L2CAP_CHAN_RAW) {
1171                 err = -EINVAL;
1172                 goto done;
1173         }
1174
1175         if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED && !(psm || cid)) {
1176                 err = -EINVAL;
1177                 goto done;
1178         }
1179
1180         switch (chan->mode) {
1181         case L2CAP_MODE_BASIC:
1182                 break;
1183         case L2CAP_MODE_ERTM:
1184         case L2CAP_MODE_STREAMING:
1185                 if (!disable_ertm)
1186                         break;
1187                 /* fall through */
1188         default:
1189                 err = -ENOTSUPP;
1190                 goto done;
1191         }
1192
1193         switch (sk->sk_state) {
1194         case BT_CONNECT:
1195         case BT_CONNECT2:
1196         case BT_CONFIG:
1197                 /* Already connecting */
1198                 err = 0;
1199                 goto done;
1200
1201         case BT_CONNECTED:
1202                 /* Already connected */
1203                 err = -EISCONN;
1204                 goto done;
1205
1206         case BT_OPEN:
1207         case BT_BOUND:
1208                 /* Can connect */
1209                 break;
1210
1211         default:
1212                 err = -EBADFD;
1213                 goto done;
1214         }
1215
1216         /* Set destination address and psm */
1217         bacpy(&bt_sk(sk)->dst, src);
1218         chan->psm = psm;
1219         chan->dcid = cid;
1220
1221         auth_type = l2cap_get_auth_type(chan);
1222
1223         if (chan->dcid == L2CAP_CID_LE_DATA)
1224                 hcon = hci_connect(hdev, LE_LINK, dst,
1225                                         chan->sec_level, auth_type);
1226         else
1227                 hcon = hci_connect(hdev, ACL_LINK, dst,
1228                                         chan->sec_level, auth_type);
1229
1230         if (IS_ERR(hcon)) {
1231                 err = PTR_ERR(hcon);
1232                 goto done;
1233         }
1234
1235         conn = l2cap_conn_add(hcon, 0);
1236         if (!conn) {
1237                 hci_conn_put(hcon);
1238                 err = -ENOMEM;
1239                 goto done;
1240         }
1241
1242         /* Update source addr of the socket */
1243         bacpy(src, conn->src);
1244
1245         l2cap_chan_add(conn, chan);
1246
1247         l2cap_state_change(chan, BT_CONNECT);
1248         __set_chan_timer(chan, sk->sk_sndtimeo);
1249
1250         if (hcon->state == BT_CONNECTED) {
1251                 if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
1252                         __clear_chan_timer(chan);
1253                         if (l2cap_chan_check_security(chan))
1254                                 l2cap_state_change(chan, BT_CONNECTED);
1255                 } else
1256                         l2cap_do_start(chan);
1257         }
1258
1259         err = 0;
1260
1261 done:
1262         hci_dev_unlock(hdev);
1263         hci_dev_put(hdev);
1264         return err;
1265 }
1266
1267 int __l2cap_wait_ack(struct sock *sk)
1268 {
1269         struct l2cap_chan *chan = l2cap_pi(sk)->chan;
1270         DECLARE_WAITQUEUE(wait, current);
1271         int err = 0;
1272         int timeo = HZ/5;
1273
1274         add_wait_queue(sk_sleep(sk), &wait);
1275         set_current_state(TASK_INTERRUPTIBLE);
1276         while (chan->unacked_frames > 0 && chan->conn) {
1277                 if (!timeo)
1278                         timeo = HZ/5;
1279
1280                 if (signal_pending(current)) {
1281                         err = sock_intr_errno(timeo);
1282                         break;
1283                 }
1284
1285                 release_sock(sk);
1286                 timeo = schedule_timeout(timeo);
1287                 lock_sock(sk);
1288                 set_current_state(TASK_INTERRUPTIBLE);
1289
1290                 err = sock_error(sk);
1291                 if (err)
1292                         break;
1293         }
1294         set_current_state(TASK_RUNNING);
1295         remove_wait_queue(sk_sleep(sk), &wait);
1296         return err;
1297 }
1298
1299 static void l2cap_monitor_timeout(struct work_struct *work)
1300 {
1301         struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
1302                                                         monitor_timer.work);
1303         struct sock *sk = chan->sk;
1304
1305         BT_DBG("chan %p", chan);
1306
1307         lock_sock(sk);
1308         if (chan->retry_count >= chan->remote_max_tx) {
1309                 l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED);
1310                 release_sock(sk);
1311                 return;
1312         }
1313
1314         chan->retry_count++;
1315         __set_monitor_timer(chan);
1316
1317         l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_POLL);
1318         release_sock(sk);
1319 }
1320
1321 static void l2cap_retrans_timeout(struct work_struct *work)
1322 {
1323         struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
1324                                                         retrans_timer.work);
1325         struct sock *sk = chan->sk;
1326
1327         BT_DBG("chan %p", chan);
1328
1329         lock_sock(sk);
1330         chan->retry_count = 1;
1331         __set_monitor_timer(chan);
1332
1333         set_bit(CONN_WAIT_F, &chan->conn_state);
1334
1335         l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_POLL);
1336         release_sock(sk);
1337 }
1338
1339 static void l2cap_drop_acked_frames(struct l2cap_chan *chan)
1340 {
1341         struct sk_buff *skb;
1342
1343         while ((skb = skb_peek(&chan->tx_q)) &&
1344                         chan->unacked_frames) {
1345                 if (bt_cb(skb)->tx_seq == chan->expected_ack_seq)
1346                         break;
1347
1348                 skb = skb_dequeue(&chan->tx_q);
1349                 kfree_skb(skb);
1350
1351                 chan->unacked_frames--;
1352         }
1353
1354         if (!chan->unacked_frames)
1355                 __clear_retrans_timer(chan);
1356 }
1357
1358 static void l2cap_streaming_send(struct l2cap_chan *chan)
1359 {
1360         struct sk_buff *skb;
1361         u32 control;
1362         u16 fcs;
1363
1364         while ((skb = skb_dequeue(&chan->tx_q))) {
1365                 control = __get_control(chan, skb->data + L2CAP_HDR_SIZE);
1366                 control |= __set_txseq(chan, chan->next_tx_seq);
1367                 __put_control(chan, control, skb->data + L2CAP_HDR_SIZE);
1368
1369                 if (chan->fcs == L2CAP_FCS_CRC16) {
1370                         fcs = crc16(0, (u8 *)skb->data,
1371                                                 skb->len - L2CAP_FCS_SIZE);
1372                         put_unaligned_le16(fcs,
1373                                         skb->data + skb->len - L2CAP_FCS_SIZE);
1374                 }
1375
1376                 l2cap_do_send(chan, skb);
1377
1378                 chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq);
1379         }
1380 }
1381
1382 static void l2cap_retransmit_one_frame(struct l2cap_chan *chan, u16 tx_seq)
1383 {
1384         struct sk_buff *skb, *tx_skb;
1385         u16 fcs;
1386         u32 control;
1387
1388         skb = skb_peek(&chan->tx_q);
1389         if (!skb)
1390                 return;
1391
1392         while (bt_cb(skb)->tx_seq != tx_seq) {
1393                 if (skb_queue_is_last(&chan->tx_q, skb))
1394                         return;
1395
1396                 skb = skb_queue_next(&chan->tx_q, skb);
1397         }
1398
1399         if (chan->remote_max_tx &&
1400                         bt_cb(skb)->retries == chan->remote_max_tx) {
1401                 l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED);
1402                 return;
1403         }
1404
1405         tx_skb = skb_clone(skb, GFP_ATOMIC);
1406         bt_cb(skb)->retries++;
1407
1408         control = __get_control(chan, tx_skb->data + L2CAP_HDR_SIZE);
1409         control &= __get_sar_mask(chan);
1410
1411         if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
1412                 control |= __set_ctrl_final(chan);
1413
1414         control |= __set_reqseq(chan, chan->buffer_seq);
1415         control |= __set_txseq(chan, tx_seq);
1416
1417         __put_control(chan, control, tx_skb->data + L2CAP_HDR_SIZE);
1418
1419         if (chan->fcs == L2CAP_FCS_CRC16) {
1420                 fcs = crc16(0, (u8 *)tx_skb->data,
1421                                                 tx_skb->len - L2CAP_FCS_SIZE);
1422                 put_unaligned_le16(fcs,
1423                                 tx_skb->data + tx_skb->len - L2CAP_FCS_SIZE);
1424         }
1425
1426         l2cap_do_send(chan, tx_skb);
1427 }
1428
1429 static int l2cap_ertm_send(struct l2cap_chan *chan)
1430 {
1431         struct sk_buff *skb, *tx_skb;
1432         u16 fcs;
1433         u32 control;
1434         int nsent = 0;
1435
1436         if (chan->state != BT_CONNECTED)
1437                 return -ENOTCONN;
1438
1439         while ((skb = chan->tx_send_head) && (!l2cap_tx_window_full(chan))) {
1440
1441                 if (chan->remote_max_tx &&
1442                                 bt_cb(skb)->retries == chan->remote_max_tx) {
1443                         l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED);
1444                         break;
1445                 }
1446
1447                 tx_skb = skb_clone(skb, GFP_ATOMIC);
1448
1449                 bt_cb(skb)->retries++;
1450
1451                 control = __get_control(chan, tx_skb->data + L2CAP_HDR_SIZE);
1452                 control &= __get_sar_mask(chan);
1453
1454                 if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
1455                         control |= __set_ctrl_final(chan);
1456
1457                 control |= __set_reqseq(chan, chan->buffer_seq);
1458                 control |= __set_txseq(chan, chan->next_tx_seq);
1459
1460                 __put_control(chan, control, tx_skb->data + L2CAP_HDR_SIZE);
1461
1462                 if (chan->fcs == L2CAP_FCS_CRC16) {
1463                         fcs = crc16(0, (u8 *)skb->data,
1464                                                 tx_skb->len - L2CAP_FCS_SIZE);
1465                         put_unaligned_le16(fcs, skb->data +
1466                                                 tx_skb->len - L2CAP_FCS_SIZE);
1467                 }
1468
1469                 l2cap_do_send(chan, tx_skb);
1470
1471                 __set_retrans_timer(chan);
1472
1473                 bt_cb(skb)->tx_seq = chan->next_tx_seq;
1474
1475                 chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq);
1476
1477                 if (bt_cb(skb)->retries == 1)
1478                         chan->unacked_frames++;
1479
1480                 chan->frames_sent++;
1481
1482                 if (skb_queue_is_last(&chan->tx_q, skb))
1483                         chan->tx_send_head = NULL;
1484                 else
1485                         chan->tx_send_head = skb_queue_next(&chan->tx_q, skb);
1486
1487                 nsent++;
1488         }
1489
1490         return nsent;
1491 }
1492
1493 static int l2cap_retransmit_frames(struct l2cap_chan *chan)
1494 {
1495         int ret;
1496
1497         if (!skb_queue_empty(&chan->tx_q))
1498                 chan->tx_send_head = chan->tx_q.next;
1499
1500         chan->next_tx_seq = chan->expected_ack_seq;
1501         ret = l2cap_ertm_send(chan);
1502         return ret;
1503 }
1504
1505 static void l2cap_send_ack(struct l2cap_chan *chan)
1506 {
1507         u32 control = 0;
1508
1509         control |= __set_reqseq(chan, chan->buffer_seq);
1510
1511         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
1512                 control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR);
1513                 set_bit(CONN_RNR_SENT, &chan->conn_state);
1514                 l2cap_send_sframe(chan, control);
1515                 return;
1516         }
1517
1518         if (l2cap_ertm_send(chan) > 0)
1519                 return;
1520
1521         control |= __set_ctrl_super(chan, L2CAP_SUPER_RR);
1522         l2cap_send_sframe(chan, control);
1523 }
1524
1525 static void l2cap_send_srejtail(struct l2cap_chan *chan)
1526 {
1527         struct srej_list *tail;
1528         u32 control;
1529
1530         control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ);
1531         control |= __set_ctrl_final(chan);
1532
1533         tail = list_entry((&chan->srej_l)->prev, struct srej_list, list);
1534         control |= __set_reqseq(chan, tail->tx_seq);
1535
1536         l2cap_send_sframe(chan, control);
1537 }
1538
1539 static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, int len, int count, struct sk_buff *skb)
1540 {
1541         struct l2cap_conn *conn = l2cap_pi(sk)->chan->conn;
1542         struct sk_buff **frag;
1543         int err, sent = 0;
1544
1545         if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count))
1546                 return -EFAULT;
1547
1548         sent += count;
1549         len  -= count;
1550
1551         /* Continuation fragments (no L2CAP header) */
1552         frag = &skb_shinfo(skb)->frag_list;
1553         while (len) {
1554                 count = min_t(unsigned int, conn->mtu, len);
1555
1556                 *frag = bt_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err);
1557                 if (!*frag)
1558                         return err;
1559                 if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count))
1560                         return -EFAULT;
1561
1562                 (*frag)->priority = skb->priority;
1563
1564                 sent += count;
1565                 len  -= count;
1566
1567                 frag = &(*frag)->next;
1568         }
1569
1570         return sent;
1571 }
1572
1573 static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan,
1574                                                 struct msghdr *msg, size_t len,
1575                                                 u32 priority)
1576 {
1577         struct sock *sk = chan->sk;
1578         struct l2cap_conn *conn = chan->conn;
1579         struct sk_buff *skb;
1580         int err, count, hlen = L2CAP_HDR_SIZE + L2CAP_PSMLEN_SIZE;
1581         struct l2cap_hdr *lh;
1582
1583         BT_DBG("sk %p len %d priority %u", sk, (int)len, priority);
1584
1585         count = min_t(unsigned int, (conn->mtu - hlen), len);
1586         skb = bt_skb_send_alloc(sk, count + hlen,
1587                         msg->msg_flags & MSG_DONTWAIT, &err);
1588         if (!skb)
1589                 return ERR_PTR(err);
1590
1591         skb->priority = priority;
1592
1593         /* Create L2CAP header */
1594         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1595         lh->cid = cpu_to_le16(chan->dcid);
1596         lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1597         put_unaligned_le16(chan->psm, skb_put(skb, 2));
1598
1599         err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1600         if (unlikely(err < 0)) {
1601                 kfree_skb(skb);
1602                 return ERR_PTR(err);
1603         }
1604         return skb;
1605 }
1606
1607 static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan,
1608                                                 struct msghdr *msg, size_t len,
1609                                                 u32 priority)
1610 {
1611         struct sock *sk = chan->sk;
1612         struct l2cap_conn *conn = chan->conn;
1613         struct sk_buff *skb;
1614         int err, count, hlen = L2CAP_HDR_SIZE;
1615         struct l2cap_hdr *lh;
1616
1617         BT_DBG("sk %p len %d", sk, (int)len);
1618
1619         count = min_t(unsigned int, (conn->mtu - hlen), len);
1620         skb = bt_skb_send_alloc(sk, count + hlen,
1621                         msg->msg_flags & MSG_DONTWAIT, &err);
1622         if (!skb)
1623                 return ERR_PTR(err);
1624
1625         skb->priority = priority;
1626
1627         /* Create L2CAP header */
1628         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1629         lh->cid = cpu_to_le16(chan->dcid);
1630         lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1631
1632         err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1633         if (unlikely(err < 0)) {
1634                 kfree_skb(skb);
1635                 return ERR_PTR(err);
1636         }
1637         return skb;
1638 }
1639
1640 static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan,
1641                                                 struct msghdr *msg, size_t len,
1642                                                 u32 control, u16 sdulen)
1643 {
1644         struct sock *sk = chan->sk;
1645         struct l2cap_conn *conn = chan->conn;
1646         struct sk_buff *skb;
1647         int err, count, hlen;
1648         struct l2cap_hdr *lh;
1649
1650         BT_DBG("sk %p len %d", sk, (int)len);
1651
1652         if (!conn)
1653                 return ERR_PTR(-ENOTCONN);
1654
1655         if (test_bit(FLAG_EXT_CTRL, &chan->flags))
1656                 hlen = L2CAP_EXT_HDR_SIZE;
1657         else
1658                 hlen = L2CAP_ENH_HDR_SIZE;
1659
1660         if (sdulen)
1661                 hlen += L2CAP_SDULEN_SIZE;
1662
1663         if (chan->fcs == L2CAP_FCS_CRC16)
1664                 hlen += L2CAP_FCS_SIZE;
1665
1666         count = min_t(unsigned int, (conn->mtu - hlen), len);
1667         skb = bt_skb_send_alloc(sk, count + hlen,
1668                         msg->msg_flags & MSG_DONTWAIT, &err);
1669         if (!skb)
1670                 return ERR_PTR(err);
1671
1672         /* Create L2CAP header */
1673         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1674         lh->cid = cpu_to_le16(chan->dcid);
1675         lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1676
1677         __put_control(chan, control, skb_put(skb, __ctrl_size(chan)));
1678
1679         if (sdulen)
1680                 put_unaligned_le16(sdulen, skb_put(skb, L2CAP_SDULEN_SIZE));
1681
1682         err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1683         if (unlikely(err < 0)) {
1684                 kfree_skb(skb);
1685                 return ERR_PTR(err);
1686         }
1687
1688         if (chan->fcs == L2CAP_FCS_CRC16)
1689                 put_unaligned_le16(0, skb_put(skb, L2CAP_FCS_SIZE));
1690
1691         bt_cb(skb)->retries = 0;
1692         return skb;
1693 }
1694
1695 static int l2cap_sar_segment_sdu(struct l2cap_chan *chan, struct msghdr *msg, size_t len)
1696 {
1697         struct sk_buff *skb;
1698         struct sk_buff_head sar_queue;
1699         u32 control;
1700         size_t size = 0;
1701
1702         skb_queue_head_init(&sar_queue);
1703         control = __set_ctrl_sar(chan, L2CAP_SAR_START);
1704         skb = l2cap_create_iframe_pdu(chan, msg, chan->remote_mps, control, len);
1705         if (IS_ERR(skb))
1706                 return PTR_ERR(skb);
1707
1708         __skb_queue_tail(&sar_queue, skb);
1709         len -= chan->remote_mps;
1710         size += chan->remote_mps;
1711
1712         while (len > 0) {
1713                 size_t buflen;
1714
1715                 if (len > chan->remote_mps) {
1716                         control = __set_ctrl_sar(chan, L2CAP_SAR_CONTINUE);
1717                         buflen = chan->remote_mps;
1718                 } else {
1719                         control = __set_ctrl_sar(chan, L2CAP_SAR_END);
1720                         buflen = len;
1721                 }
1722
1723                 skb = l2cap_create_iframe_pdu(chan, msg, buflen, control, 0);
1724                 if (IS_ERR(skb)) {
1725                         skb_queue_purge(&sar_queue);
1726                         return PTR_ERR(skb);
1727                 }
1728
1729                 __skb_queue_tail(&sar_queue, skb);
1730                 len -= buflen;
1731                 size += buflen;
1732         }
1733         skb_queue_splice_tail(&sar_queue, &chan->tx_q);
1734         if (chan->tx_send_head == NULL)
1735                 chan->tx_send_head = sar_queue.next;
1736
1737         return size;
1738 }
1739
1740 int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len,
1741                                                                 u32 priority)
1742 {
1743         struct sk_buff *skb;
1744         u32 control;
1745         int err;
1746
1747         /* Connectionless channel */
1748         if (chan->chan_type == L2CAP_CHAN_CONN_LESS) {
1749                 skb = l2cap_create_connless_pdu(chan, msg, len, priority);
1750                 if (IS_ERR(skb))
1751                         return PTR_ERR(skb);
1752
1753                 l2cap_do_send(chan, skb);
1754                 return len;
1755         }
1756
1757         switch (chan->mode) {
1758         case L2CAP_MODE_BASIC:
1759                 /* Check outgoing MTU */
1760                 if (len > chan->omtu)
1761                         return -EMSGSIZE;
1762
1763                 /* Create a basic PDU */
1764                 skb = l2cap_create_basic_pdu(chan, msg, len, priority);
1765                 if (IS_ERR(skb))
1766                         return PTR_ERR(skb);
1767
1768                 l2cap_do_send(chan, skb);
1769                 err = len;
1770                 break;
1771
1772         case L2CAP_MODE_ERTM:
1773         case L2CAP_MODE_STREAMING:
1774                 /* Entire SDU fits into one PDU */
1775                 if (len <= chan->remote_mps) {
1776                         control = __set_ctrl_sar(chan, L2CAP_SAR_UNSEGMENTED);
1777                         skb = l2cap_create_iframe_pdu(chan, msg, len, control,
1778                                                                         0);
1779                         if (IS_ERR(skb))
1780                                 return PTR_ERR(skb);
1781
1782                         __skb_queue_tail(&chan->tx_q, skb);
1783
1784                         if (chan->tx_send_head == NULL)
1785                                 chan->tx_send_head = skb;
1786
1787                 } else {
1788                         /* Segment SDU into multiples PDUs */
1789                         err = l2cap_sar_segment_sdu(chan, msg, len);
1790                         if (err < 0)
1791                                 return err;
1792                 }
1793
1794                 if (chan->mode == L2CAP_MODE_STREAMING) {
1795                         l2cap_streaming_send(chan);
1796                         err = len;
1797                         break;
1798                 }
1799
1800                 if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
1801                                 test_bit(CONN_WAIT_F, &chan->conn_state)) {
1802                         err = len;
1803                         break;
1804                 }
1805
1806                 err = l2cap_ertm_send(chan);
1807                 if (err >= 0)
1808                         err = len;
1809
1810                 break;
1811
1812         default:
1813                 BT_DBG("bad state %1.1x", chan->mode);
1814                 err = -EBADFD;
1815         }
1816
1817         return err;
1818 }
1819
1820 /* Copy frame to all raw sockets on that connection */
1821 static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb)
1822 {
1823         struct sk_buff *nskb;
1824         struct l2cap_chan *chan;
1825
1826         BT_DBG("conn %p", conn);
1827
1828         mutex_lock(&conn->chan_lock);
1829         list_for_each_entry(chan, &conn->chan_l, list) {
1830                 struct sock *sk = chan->sk;
1831                 if (chan->chan_type != L2CAP_CHAN_RAW)
1832                         continue;
1833
1834                 /* Don't send frame to the socket it came from */
1835                 if (skb->sk == sk)
1836                         continue;
1837                 nskb = skb_clone(skb, GFP_ATOMIC);
1838                 if (!nskb)
1839                         continue;
1840
1841                 if (chan->ops->recv(chan->data, nskb))
1842                         kfree_skb(nskb);
1843         }
1844         mutex_unlock(&conn->chan_lock);
1845 }
1846
1847 /* ---- L2CAP signalling commands ---- */
1848 static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
1849                                 u8 code, u8 ident, u16 dlen, void *data)
1850 {
1851         struct sk_buff *skb, **frag;
1852         struct l2cap_cmd_hdr *cmd;
1853         struct l2cap_hdr *lh;
1854         int len, count;
1855
1856         BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %d",
1857                         conn, code, ident, dlen);
1858
1859         len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen;
1860         count = min_t(unsigned int, conn->mtu, len);
1861
1862         skb = bt_skb_alloc(count, GFP_ATOMIC);
1863         if (!skb)
1864                 return NULL;
1865
1866         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1867         lh->len = cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen);
1868
1869         if (conn->hcon->type == LE_LINK)
1870                 lh->cid = cpu_to_le16(L2CAP_CID_LE_SIGNALING);
1871         else
1872                 lh->cid = cpu_to_le16(L2CAP_CID_SIGNALING);
1873
1874         cmd = (struct l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE);
1875         cmd->code  = code;
1876         cmd->ident = ident;
1877         cmd->len   = cpu_to_le16(dlen);
1878
1879         if (dlen) {
1880                 count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE;
1881                 memcpy(skb_put(skb, count), data, count);
1882                 data += count;
1883         }
1884
1885         len -= skb->len;
1886
1887         /* Continuation fragments (no L2CAP header) */
1888         frag = &skb_shinfo(skb)->frag_list;
1889         while (len) {
1890                 count = min_t(unsigned int, conn->mtu, len);
1891
1892                 *frag = bt_skb_alloc(count, GFP_ATOMIC);
1893                 if (!*frag)
1894                         goto fail;
1895
1896                 memcpy(skb_put(*frag, count), data, count);
1897
1898                 len  -= count;
1899                 data += count;
1900
1901                 frag = &(*frag)->next;
1902         }
1903
1904         return skb;
1905
1906 fail:
1907         kfree_skb(skb);
1908         return NULL;
1909 }
1910
1911 static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, unsigned long *val)
1912 {
1913         struct l2cap_conf_opt *opt = *ptr;
1914         int len;
1915
1916         len = L2CAP_CONF_OPT_SIZE + opt->len;
1917         *ptr += len;
1918
1919         *type = opt->type;
1920         *olen = opt->len;
1921
1922         switch (opt->len) {
1923         case 1:
1924                 *val = *((u8 *) opt->val);
1925                 break;
1926
1927         case 2:
1928                 *val = get_unaligned_le16(opt->val);
1929                 break;
1930
1931         case 4:
1932                 *val = get_unaligned_le32(opt->val);
1933                 break;
1934
1935         default:
1936                 *val = (unsigned long) opt->val;
1937                 break;
1938         }
1939
1940         BT_DBG("type 0x%2.2x len %d val 0x%lx", *type, opt->len, *val);
1941         return len;
1942 }
1943
1944 static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val)
1945 {
1946         struct l2cap_conf_opt *opt = *ptr;
1947
1948         BT_DBG("type 0x%2.2x len %d val 0x%lx", type, len, val);
1949
1950         opt->type = type;
1951         opt->len  = len;
1952
1953         switch (len) {
1954         case 1:
1955                 *((u8 *) opt->val)  = val;
1956                 break;
1957
1958         case 2:
1959                 put_unaligned_le16(val, opt->val);
1960                 break;
1961
1962         case 4:
1963                 put_unaligned_le32(val, opt->val);
1964                 break;
1965
1966         default:
1967                 memcpy(opt->val, (void *) val, len);
1968                 break;
1969         }
1970
1971         *ptr += L2CAP_CONF_OPT_SIZE + len;
1972 }
1973
1974 static void l2cap_add_opt_efs(void **ptr, struct l2cap_chan *chan)
1975 {
1976         struct l2cap_conf_efs efs;
1977
1978         switch (chan->mode) {
1979         case L2CAP_MODE_ERTM:
1980                 efs.id          = chan->local_id;
1981                 efs.stype       = chan->local_stype;
1982                 efs.msdu        = cpu_to_le16(chan->local_msdu);
1983                 efs.sdu_itime   = cpu_to_le32(chan->local_sdu_itime);
1984                 efs.acc_lat     = cpu_to_le32(L2CAP_DEFAULT_ACC_LAT);
1985                 efs.flush_to    = cpu_to_le32(L2CAP_DEFAULT_FLUSH_TO);
1986                 break;
1987
1988         case L2CAP_MODE_STREAMING:
1989                 efs.id          = 1;
1990                 efs.stype       = L2CAP_SERV_BESTEFFORT;
1991                 efs.msdu        = cpu_to_le16(chan->local_msdu);
1992                 efs.sdu_itime   = cpu_to_le32(chan->local_sdu_itime);
1993                 efs.acc_lat     = 0;
1994                 efs.flush_to    = 0;
1995                 break;
1996
1997         default:
1998                 return;
1999         }
2000
2001         l2cap_add_conf_opt(ptr, L2CAP_CONF_EFS, sizeof(efs),
2002                                                         (unsigned long) &efs);
2003 }
2004
2005 static void l2cap_ack_timeout(struct work_struct *work)
2006 {
2007         struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
2008                                                         ack_timer.work);
2009
2010         lock_sock(chan->sk);
2011         l2cap_send_ack(chan);
2012         release_sock(chan->sk);
2013 }
2014
2015 static inline void l2cap_ertm_init(struct l2cap_chan *chan)
2016 {
2017         chan->expected_ack_seq = 0;
2018         chan->unacked_frames = 0;
2019         chan->buffer_seq = 0;
2020         chan->num_acked = 0;
2021         chan->frames_sent = 0;
2022
2023         INIT_DELAYED_WORK(&chan->retrans_timer, l2cap_retrans_timeout);
2024         INIT_DELAYED_WORK(&chan->monitor_timer, l2cap_monitor_timeout);
2025         INIT_DELAYED_WORK(&chan->ack_timer, l2cap_ack_timeout);
2026
2027         skb_queue_head_init(&chan->srej_q);
2028
2029         INIT_LIST_HEAD(&chan->srej_l);
2030 }
2031
2032 static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask)
2033 {
2034         switch (mode) {
2035         case L2CAP_MODE_STREAMING:
2036         case L2CAP_MODE_ERTM:
2037                 if (l2cap_mode_supported(mode, remote_feat_mask))
2038                         return mode;
2039                 /* fall through */
2040         default:
2041                 return L2CAP_MODE_BASIC;
2042         }
2043 }
2044
2045 static inline bool __l2cap_ews_supported(struct l2cap_chan *chan)
2046 {
2047         return enable_hs && chan->conn->feat_mask & L2CAP_FEAT_EXT_WINDOW;
2048 }
2049
2050 static inline bool __l2cap_efs_supported(struct l2cap_chan *chan)
2051 {
2052         return enable_hs && chan->conn->feat_mask & L2CAP_FEAT_EXT_FLOW;
2053 }
2054
2055 static inline void l2cap_txwin_setup(struct l2cap_chan *chan)
2056 {
2057         if (chan->tx_win > L2CAP_DEFAULT_TX_WINDOW &&
2058                                                 __l2cap_ews_supported(chan)) {
2059                 /* use extended control field */
2060                 set_bit(FLAG_EXT_CTRL, &chan->flags);
2061                 chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
2062         } else {
2063                 chan->tx_win = min_t(u16, chan->tx_win,
2064                                                 L2CAP_DEFAULT_TX_WINDOW);
2065                 chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW;
2066         }
2067 }
2068
2069 static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data)
2070 {
2071         struct l2cap_conf_req *req = data;
2072         struct l2cap_conf_rfc rfc = { .mode = chan->mode };
2073         void *ptr = req->data;
2074         u16 size;
2075
2076         BT_DBG("chan %p", chan);
2077
2078         if (chan->num_conf_req || chan->num_conf_rsp)
2079                 goto done;
2080
2081         switch (chan->mode) {
2082         case L2CAP_MODE_STREAMING:
2083         case L2CAP_MODE_ERTM:
2084                 if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state))
2085                         break;
2086
2087                 if (__l2cap_efs_supported(chan))
2088                         set_bit(FLAG_EFS_ENABLE, &chan->flags);
2089
2090                 /* fall through */
2091         default:
2092                 chan->mode = l2cap_select_mode(rfc.mode, chan->conn->feat_mask);
2093                 break;
2094         }
2095
2096 done:
2097         if (chan->imtu != L2CAP_DEFAULT_MTU)
2098                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu);
2099
2100         switch (chan->mode) {
2101         case L2CAP_MODE_BASIC:
2102                 if (!(chan->conn->feat_mask & L2CAP_FEAT_ERTM) &&
2103                                 !(chan->conn->feat_mask & L2CAP_FEAT_STREAMING))
2104                         break;
2105
2106                 rfc.mode            = L2CAP_MODE_BASIC;
2107                 rfc.txwin_size      = 0;
2108                 rfc.max_transmit    = 0;
2109                 rfc.retrans_timeout = 0;
2110                 rfc.monitor_timeout = 0;
2111                 rfc.max_pdu_size    = 0;
2112
2113                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
2114                                                         (unsigned long) &rfc);
2115                 break;
2116
2117         case L2CAP_MODE_ERTM:
2118                 rfc.mode            = L2CAP_MODE_ERTM;
2119                 rfc.max_transmit    = chan->max_tx;
2120                 rfc.retrans_timeout = 0;
2121                 rfc.monitor_timeout = 0;
2122
2123                 size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
2124                                                 L2CAP_EXT_HDR_SIZE -
2125                                                 L2CAP_SDULEN_SIZE -
2126                                                 L2CAP_FCS_SIZE);
2127                 rfc.max_pdu_size = cpu_to_le16(size);
2128
2129                 l2cap_txwin_setup(chan);
2130
2131                 rfc.txwin_size = min_t(u16, chan->tx_win,
2132                                                 L2CAP_DEFAULT_TX_WINDOW);
2133
2134                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
2135                                                         (unsigned long) &rfc);
2136
2137                 if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
2138                         l2cap_add_opt_efs(&ptr, chan);
2139
2140                 if (!(chan->conn->feat_mask & L2CAP_FEAT_FCS))
2141                         break;
2142
2143                 if (chan->fcs == L2CAP_FCS_NONE ||
2144                                 test_bit(CONF_NO_FCS_RECV, &chan->conf_state)) {
2145                         chan->fcs = L2CAP_FCS_NONE;
2146                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, chan->fcs);
2147                 }
2148
2149                 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
2150                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
2151                                                                 chan->tx_win);
2152                 break;
2153
2154         case L2CAP_MODE_STREAMING:
2155                 rfc.mode            = L2CAP_MODE_STREAMING;
2156                 rfc.txwin_size      = 0;
2157                 rfc.max_transmit    = 0;
2158                 rfc.retrans_timeout = 0;
2159                 rfc.monitor_timeout = 0;
2160
2161                 size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
2162                                                 L2CAP_EXT_HDR_SIZE -
2163                                                 L2CAP_SDULEN_SIZE -
2164                                                 L2CAP_FCS_SIZE);
2165                 rfc.max_pdu_size = cpu_to_le16(size);
2166
2167                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
2168                                                         (unsigned long) &rfc);
2169
2170                 if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
2171                         l2cap_add_opt_efs(&ptr, chan);
2172
2173                 if (!(chan->conn->feat_mask & L2CAP_FEAT_FCS))
2174                         break;
2175
2176                 if (chan->fcs == L2CAP_FCS_NONE ||
2177                                 test_bit(CONF_NO_FCS_RECV, &chan->conf_state)) {
2178                         chan->fcs = L2CAP_FCS_NONE;
2179                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, chan->fcs);
2180                 }
2181                 break;
2182         }
2183
2184         req->dcid  = cpu_to_le16(chan->dcid);
2185         req->flags = cpu_to_le16(0);
2186
2187         return ptr - data;
2188 }
2189
2190 static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data)
2191 {
2192         struct l2cap_conf_rsp *rsp = data;
2193         void *ptr = rsp->data;
2194         void *req = chan->conf_req;
2195         int len = chan->conf_len;
2196         int type, hint, olen;
2197         unsigned long val;
2198         struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
2199         struct l2cap_conf_efs efs;
2200         u8 remote_efs = 0;
2201         u16 mtu = L2CAP_DEFAULT_MTU;
2202         u16 result = L2CAP_CONF_SUCCESS;
2203         u16 size;
2204
2205         BT_DBG("chan %p", chan);
2206
2207         while (len >= L2CAP_CONF_OPT_SIZE) {
2208                 len -= l2cap_get_conf_opt(&req, &type, &olen, &val);
2209
2210                 hint  = type & L2CAP_CONF_HINT;
2211                 type &= L2CAP_CONF_MASK;
2212
2213                 switch (type) {
2214                 case L2CAP_CONF_MTU:
2215                         mtu = val;
2216                         break;
2217
2218                 case L2CAP_CONF_FLUSH_TO:
2219                         chan->flush_to = val;
2220                         break;
2221
2222                 case L2CAP_CONF_QOS:
2223                         break;
2224
2225                 case L2CAP_CONF_RFC:
2226                         if (olen == sizeof(rfc))
2227                                 memcpy(&rfc, (void *) val, olen);
2228                         break;
2229
2230                 case L2CAP_CONF_FCS:
2231                         if (val == L2CAP_FCS_NONE)
2232                                 set_bit(CONF_NO_FCS_RECV, &chan->conf_state);
2233                         break;
2234
2235                 case L2CAP_CONF_EFS:
2236                         remote_efs = 1;
2237                         if (olen == sizeof(efs))
2238                                 memcpy(&efs, (void *) val, olen);
2239                         break;
2240
2241                 case L2CAP_CONF_EWS:
2242                         if (!enable_hs)
2243                                 return -ECONNREFUSED;
2244
2245                         set_bit(FLAG_EXT_CTRL, &chan->flags);
2246                         set_bit(CONF_EWS_RECV, &chan->conf_state);
2247                         chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
2248                         chan->remote_tx_win = val;
2249                         break;
2250
2251                 default:
2252                         if (hint)
2253                                 break;
2254
2255                         result = L2CAP_CONF_UNKNOWN;
2256                         *((u8 *) ptr++) = type;
2257                         break;
2258                 }
2259         }
2260
2261         if (chan->num_conf_rsp || chan->num_conf_req > 1)
2262                 goto done;
2263
2264         switch (chan->mode) {
2265         case L2CAP_MODE_STREAMING:
2266         case L2CAP_MODE_ERTM:
2267                 if (!test_bit(CONF_STATE2_DEVICE, &chan->conf_state)) {
2268                         chan->mode = l2cap_select_mode(rfc.mode,
2269                                         chan->conn->feat_mask);
2270                         break;
2271                 }
2272
2273                 if (remote_efs) {
2274                         if (__l2cap_efs_supported(chan))
2275                                 set_bit(FLAG_EFS_ENABLE, &chan->flags);
2276                         else
2277                                 return -ECONNREFUSED;
2278                 }
2279
2280                 if (chan->mode != rfc.mode)
2281                         return -ECONNREFUSED;
2282
2283                 break;
2284         }
2285
2286 done:
2287         if (chan->mode != rfc.mode) {
2288                 result = L2CAP_CONF_UNACCEPT;
2289                 rfc.mode = chan->mode;
2290
2291                 if (chan->num_conf_rsp == 1)
2292                         return -ECONNREFUSED;
2293
2294                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2295                                         sizeof(rfc), (unsigned long) &rfc);
2296         }
2297
2298         if (result == L2CAP_CONF_SUCCESS) {
2299                 /* Configure output options and let the other side know
2300                  * which ones we don't like. */
2301
2302                 if (mtu < L2CAP_DEFAULT_MIN_MTU)
2303                         result = L2CAP_CONF_UNACCEPT;
2304                 else {
2305                         chan->omtu = mtu;
2306                         set_bit(CONF_MTU_DONE, &chan->conf_state);
2307                 }
2308                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->omtu);
2309
2310                 if (remote_efs) {
2311                         if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
2312                                         efs.stype != L2CAP_SERV_NOTRAFIC &&
2313                                         efs.stype != chan->local_stype) {
2314
2315                                 result = L2CAP_CONF_UNACCEPT;
2316
2317                                 if (chan->num_conf_req >= 1)
2318                                         return -ECONNREFUSED;
2319
2320                                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
2321                                                         sizeof(efs),
2322                                                         (unsigned long) &efs);
2323                         } else {
2324                                 /* Send PENDING Conf Rsp */
2325                                 result = L2CAP_CONF_PENDING;
2326                                 set_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
2327                         }
2328                 }
2329
2330                 switch (rfc.mode) {
2331                 case L2CAP_MODE_BASIC:
2332                         chan->fcs = L2CAP_FCS_NONE;
2333                         set_bit(CONF_MODE_DONE, &chan->conf_state);
2334                         break;
2335
2336                 case L2CAP_MODE_ERTM:
2337                         if (!test_bit(CONF_EWS_RECV, &chan->conf_state))
2338                                 chan->remote_tx_win = rfc.txwin_size;
2339                         else
2340                                 rfc.txwin_size = L2CAP_DEFAULT_TX_WINDOW;
2341
2342                         chan->remote_max_tx = rfc.max_transmit;
2343
2344                         size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
2345                                                 chan->conn->mtu -
2346                                                 L2CAP_EXT_HDR_SIZE -
2347                                                 L2CAP_SDULEN_SIZE -
2348                                                 L2CAP_FCS_SIZE);
2349                         rfc.max_pdu_size = cpu_to_le16(size);
2350                         chan->remote_mps = size;
2351
2352                         rfc.retrans_timeout =
2353                                 le16_to_cpu(L2CAP_DEFAULT_RETRANS_TO);
2354                         rfc.monitor_timeout =
2355                                 le16_to_cpu(L2CAP_DEFAULT_MONITOR_TO);
2356
2357                         set_bit(CONF_MODE_DONE, &chan->conf_state);
2358
2359                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2360                                         sizeof(rfc), (unsigned long) &rfc);
2361
2362                         if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
2363                                 chan->remote_id = efs.id;
2364                                 chan->remote_stype = efs.stype;
2365                                 chan->remote_msdu = le16_to_cpu(efs.msdu);
2366                                 chan->remote_flush_to =
2367                                                 le32_to_cpu(efs.flush_to);
2368                                 chan->remote_acc_lat =
2369                                                 le32_to_cpu(efs.acc_lat);
2370                                 chan->remote_sdu_itime =
2371                                         le32_to_cpu(efs.sdu_itime);
2372                                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
2373                                         sizeof(efs), (unsigned long) &efs);
2374                         }
2375                         break;
2376
2377                 case L2CAP_MODE_STREAMING:
2378                         size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
2379                                                 chan->conn->mtu -
2380                                                 L2CAP_EXT_HDR_SIZE -
2381                                                 L2CAP_SDULEN_SIZE -
2382                                                 L2CAP_FCS_SIZE);
2383                         rfc.max_pdu_size = cpu_to_le16(size);
2384                         chan->remote_mps = size;
2385
2386                         set_bit(CONF_MODE_DONE, &chan->conf_state);
2387
2388                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2389                                         sizeof(rfc), (unsigned long) &rfc);
2390
2391                         break;
2392
2393                 default:
2394                         result = L2CAP_CONF_UNACCEPT;
2395
2396                         memset(&rfc, 0, sizeof(rfc));
2397                         rfc.mode = chan->mode;
2398                 }
2399
2400                 if (result == L2CAP_CONF_SUCCESS)
2401                         set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
2402         }
2403         rsp->scid   = cpu_to_le16(chan->dcid);
2404         rsp->result = cpu_to_le16(result);
2405         rsp->flags  = cpu_to_le16(0x0000);
2406
2407         return ptr - data;
2408 }
2409
2410 static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, void *data, u16 *result)
2411 {
2412         struct l2cap_conf_req *req = data;
2413         void *ptr = req->data;
2414         int type, olen;
2415         unsigned long val;
2416         struct l2cap_conf_rfc rfc;
2417         struct l2cap_conf_efs efs;
2418
2419         BT_DBG("chan %p, rsp %p, len %d, req %p", chan, rsp, len, data);
2420
2421         while (len >= L2CAP_CONF_OPT_SIZE) {
2422                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
2423
2424                 switch (type) {
2425                 case L2CAP_CONF_MTU:
2426                         if (val < L2CAP_DEFAULT_MIN_MTU) {
2427                                 *result = L2CAP_CONF_UNACCEPT;
2428                                 chan->imtu = L2CAP_DEFAULT_MIN_MTU;
2429                         } else
2430                                 chan->imtu = val;
2431                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu);
2432                         break;
2433
2434                 case L2CAP_CONF_FLUSH_TO:
2435                         chan->flush_to = val;
2436                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
2437                                                         2, chan->flush_to);
2438                         break;
2439
2440                 case L2CAP_CONF_RFC:
2441                         if (olen == sizeof(rfc))
2442                                 memcpy(&rfc, (void *)val, olen);
2443
2444                         if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state) &&
2445                                                         rfc.mode != chan->mode)
2446                                 return -ECONNREFUSED;
2447
2448                         chan->fcs = 0;
2449
2450                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2451                                         sizeof(rfc), (unsigned long) &rfc);
2452                         break;
2453
2454                 case L2CAP_CONF_EWS:
2455                         chan->tx_win = min_t(u16, val,
2456                                                 L2CAP_DEFAULT_EXT_WINDOW);
2457                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
2458                                                         chan->tx_win);
2459                         break;
2460
2461                 case L2CAP_CONF_EFS:
2462                         if (olen == sizeof(efs))
2463                                 memcpy(&efs, (void *)val, olen);
2464
2465                         if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
2466                                         efs.stype != L2CAP_SERV_NOTRAFIC &&
2467                                         efs.stype != chan->local_stype)
2468                                 return -ECONNREFUSED;
2469
2470                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
2471                                         sizeof(efs), (unsigned long) &efs);
2472                         break;
2473                 }
2474         }
2475
2476         if (chan->mode == L2CAP_MODE_BASIC && chan->mode != rfc.mode)
2477                 return -ECONNREFUSED;
2478
2479         chan->mode = rfc.mode;
2480
2481         if (*result == L2CAP_CONF_SUCCESS || *result == L2CAP_CONF_PENDING) {
2482                 switch (rfc.mode) {
2483                 case L2CAP_MODE_ERTM:
2484                         chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
2485                         chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
2486                         chan->mps    = le16_to_cpu(rfc.max_pdu_size);
2487
2488                         if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
2489                                 chan->local_msdu = le16_to_cpu(efs.msdu);
2490                                 chan->local_sdu_itime =
2491                                                 le32_to_cpu(efs.sdu_itime);
2492                                 chan->local_acc_lat = le32_to_cpu(efs.acc_lat);
2493                                 chan->local_flush_to =
2494                                                 le32_to_cpu(efs.flush_to);
2495                         }
2496                         break;
2497
2498                 case L2CAP_MODE_STREAMING:
2499                         chan->mps    = le16_to_cpu(rfc.max_pdu_size);
2500                 }
2501         }
2502
2503         req->dcid   = cpu_to_le16(chan->dcid);
2504         req->flags  = cpu_to_le16(0x0000);
2505
2506         return ptr - data;
2507 }
2508
2509 static int l2cap_build_conf_rsp(struct l2cap_chan *chan, void *data, u16 result, u16 flags)
2510 {
2511         struct l2cap_conf_rsp *rsp = data;
2512         void *ptr = rsp->data;
2513
2514         BT_DBG("chan %p", chan);
2515
2516         rsp->scid   = cpu_to_le16(chan->dcid);
2517         rsp->result = cpu_to_le16(result);
2518         rsp->flags  = cpu_to_le16(flags);
2519
2520         return ptr - data;
2521 }
2522
2523 void __l2cap_connect_rsp_defer(struct l2cap_chan *chan)
2524 {
2525         struct l2cap_conn_rsp rsp;
2526         struct l2cap_conn *conn = chan->conn;
2527         u8 buf[128];
2528
2529         rsp.scid   = cpu_to_le16(chan->dcid);
2530         rsp.dcid   = cpu_to_le16(chan->scid);
2531         rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
2532         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
2533         l2cap_send_cmd(conn, chan->ident,
2534                                 L2CAP_CONN_RSP, sizeof(rsp), &rsp);
2535
2536         if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
2537                 return;
2538
2539         l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2540                         l2cap_build_conf_req(chan, buf), buf);
2541         chan->num_conf_req++;
2542 }
2543
2544 static void l2cap_conf_rfc_get(struct l2cap_chan *chan, void *rsp, int len)
2545 {
2546         int type, olen;
2547         unsigned long val;
2548         struct l2cap_conf_rfc rfc;
2549
2550         BT_DBG("chan %p, rsp %p, len %d", chan, rsp, len);
2551
2552         if ((chan->mode != L2CAP_MODE_ERTM) && (chan->mode != L2CAP_MODE_STREAMING))
2553                 return;
2554
2555         while (len >= L2CAP_CONF_OPT_SIZE) {
2556                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
2557
2558                 switch (type) {
2559                 case L2CAP_CONF_RFC:
2560                         if (olen == sizeof(rfc))
2561                                 memcpy(&rfc, (void *)val, olen);
2562                         goto done;
2563                 }
2564         }
2565
2566 done:
2567         switch (rfc.mode) {
2568         case L2CAP_MODE_ERTM:
2569                 chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
2570                 chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
2571                 chan->mps    = le16_to_cpu(rfc.max_pdu_size);
2572                 break;
2573         case L2CAP_MODE_STREAMING:
2574                 chan->mps    = le16_to_cpu(rfc.max_pdu_size);
2575         }
2576 }
2577
2578 static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2579 {
2580         struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data;
2581
2582         if (rej->reason != L2CAP_REJ_NOT_UNDERSTOOD)
2583                 return 0;
2584
2585         if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
2586                                         cmd->ident == conn->info_ident) {
2587                 cancel_delayed_work_sync(&conn->info_work);
2588
2589                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2590                 conn->info_ident = 0;
2591
2592                 l2cap_conn_start(conn);
2593         }
2594
2595         return 0;
2596 }
2597
2598 static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2599 {
2600         struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
2601         struct l2cap_conn_rsp rsp;
2602         struct l2cap_chan *chan = NULL, *pchan;
2603         struct sock *parent, *sk = NULL;
2604         int result, status = L2CAP_CS_NO_INFO;
2605
2606         u16 dcid = 0, scid = __le16_to_cpu(req->scid);
2607         __le16 psm = req->psm;
2608
2609         BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid);
2610
2611         /* Check if we have socket listening on psm */
2612         pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, conn->src);
2613         if (!pchan) {
2614                 result = L2CAP_CR_BAD_PSM;
2615                 goto sendresp;
2616         }
2617
2618         parent = pchan->sk;
2619
2620         lock_sock(parent);
2621
2622         /* Check if the ACL is secure enough (if not SDP) */
2623         if (psm != cpu_to_le16(0x0001) &&
2624                                 !hci_conn_check_link_mode(conn->hcon)) {
2625                 conn->disc_reason = HCI_ERROR_AUTH_FAILURE;
2626                 result = L2CAP_CR_SEC_BLOCK;
2627                 goto response;
2628         }
2629
2630         result = L2CAP_CR_NO_MEM;
2631
2632         /* Check for backlog size */
2633         if (sk_acceptq_is_full(parent)) {
2634                 BT_DBG("backlog full %d", parent->sk_ack_backlog);
2635                 goto response;
2636         }
2637
2638         chan = pchan->ops->new_connection(pchan->data);
2639         if (!chan)
2640                 goto response;
2641
2642         sk = chan->sk;
2643
2644         mutex_lock(&conn->chan_lock);
2645
2646         /* Check if we already have channel with that dcid */
2647         if (__l2cap_get_chan_by_dcid(conn, scid)) {
2648                 mutex_unlock(&conn->chan_lock);
2649                 sock_set_flag(sk, SOCK_ZAPPED);
2650                 chan->ops->close(chan->data);
2651                 goto response;
2652         }
2653
2654         hci_conn_hold(conn->hcon);
2655
2656         bacpy(&bt_sk(sk)->src, conn->src);
2657         bacpy(&bt_sk(sk)->dst, conn->dst);
2658         chan->psm  = psm;
2659         chan->dcid = scid;
2660
2661         bt_accept_enqueue(parent, sk);
2662
2663         __l2cap_chan_add(conn, chan);
2664
2665         dcid = chan->scid;
2666
2667         __set_chan_timer(chan, sk->sk_sndtimeo);
2668
2669         chan->ident = cmd->ident;
2670
2671         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
2672                 if (l2cap_chan_check_security(chan)) {
2673                         if (bt_sk(sk)->defer_setup) {
2674                                 l2cap_state_change(chan, BT_CONNECT2);
2675                                 result = L2CAP_CR_PEND;
2676                                 status = L2CAP_CS_AUTHOR_PEND;
2677                                 parent->sk_data_ready(parent, 0);
2678                         } else {
2679                                 l2cap_state_change(chan, BT_CONFIG);
2680                                 result = L2CAP_CR_SUCCESS;
2681                                 status = L2CAP_CS_NO_INFO;
2682                         }
2683                 } else {
2684                         l2cap_state_change(chan, BT_CONNECT2);
2685                         result = L2CAP_CR_PEND;
2686                         status = L2CAP_CS_AUTHEN_PEND;
2687                 }
2688         } else {
2689                 l2cap_state_change(chan, BT_CONNECT2);
2690                 result = L2CAP_CR_PEND;
2691                 status = L2CAP_CS_NO_INFO;
2692         }
2693
2694         mutex_unlock(&conn->chan_lock);
2695
2696 response:
2697         release_sock(parent);
2698
2699 sendresp:
2700         rsp.scid   = cpu_to_le16(scid);
2701         rsp.dcid   = cpu_to_le16(dcid);
2702         rsp.result = cpu_to_le16(result);
2703         rsp.status = cpu_to_le16(status);
2704         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
2705
2706         if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
2707                 struct l2cap_info_req info;
2708                 info.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
2709
2710                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
2711                 conn->info_ident = l2cap_get_ident(conn);
2712
2713                 schedule_delayed_work(&conn->info_work,
2714                                         msecs_to_jiffies(L2CAP_INFO_TIMEOUT));
2715
2716                 l2cap_send_cmd(conn, conn->info_ident,
2717                                         L2CAP_INFO_REQ, sizeof(info), &info);
2718         }
2719
2720         if (chan && !test_bit(CONF_REQ_SENT, &chan->conf_state) &&
2721                                 result == L2CAP_CR_SUCCESS) {
2722                 u8 buf[128];
2723                 set_bit(CONF_REQ_SENT, &chan->conf_state);
2724                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2725                                         l2cap_build_conf_req(chan, buf), buf);
2726                 chan->num_conf_req++;
2727         }
2728
2729         return 0;
2730 }
2731
2732 static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2733 {
2734         struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
2735         u16 scid, dcid, result, status;
2736         struct l2cap_chan *chan;
2737         struct sock *sk;
2738         u8 req[128];
2739
2740         scid   = __le16_to_cpu(rsp->scid);
2741         dcid   = __le16_to_cpu(rsp->dcid);
2742         result = __le16_to_cpu(rsp->result);
2743         status = __le16_to_cpu(rsp->status);
2744
2745         BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", dcid, scid, result, status);
2746
2747         if (scid) {
2748                 chan = l2cap_get_chan_by_scid(conn, scid);
2749                 if (!chan)
2750                         return -EFAULT;
2751         } else {
2752                 chan = l2cap_get_chan_by_ident(conn, cmd->ident);
2753                 if (!chan)
2754                         return -EFAULT;
2755         }
2756
2757         sk = chan->sk;
2758
2759         switch (result) {
2760         case L2CAP_CR_SUCCESS:
2761                 l2cap_state_change(chan, BT_CONFIG);
2762                 chan->ident = 0;
2763                 chan->dcid = dcid;
2764                 clear_bit(CONF_CONNECT_PEND, &chan->conf_state);
2765
2766                 if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
2767                         break;
2768
2769                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2770                                         l2cap_build_conf_req(chan, req), req);
2771                 chan->num_conf_req++;
2772                 break;
2773
2774         case L2CAP_CR_PEND:
2775                 set_bit(CONF_CONNECT_PEND, &chan->conf_state);
2776                 break;
2777
2778         default:
2779                 l2cap_chan_del(chan, ECONNREFUSED);
2780                 break;
2781         }
2782
2783         release_sock(sk);
2784         return 0;
2785 }
2786
2787 static inline void set_default_fcs(struct l2cap_chan *chan)
2788 {
2789         /* FCS is enabled only in ERTM or streaming mode, if one or both
2790          * sides request it.
2791          */
2792         if (chan->mode != L2CAP_MODE_ERTM && chan->mode != L2CAP_MODE_STREAMING)
2793                 chan->fcs = L2CAP_FCS_NONE;
2794         else if (!test_bit(CONF_NO_FCS_RECV, &chan->conf_state))
2795                 chan->fcs = L2CAP_FCS_CRC16;
2796 }
2797
2798 static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
2799 {
2800         struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
2801         u16 dcid, flags;
2802         u8 rsp[64];
2803         struct l2cap_chan *chan;
2804         struct sock *sk;
2805         int len;
2806
2807         dcid  = __le16_to_cpu(req->dcid);
2808         flags = __le16_to_cpu(req->flags);
2809
2810         BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
2811
2812         chan = l2cap_get_chan_by_scid(conn, dcid);
2813         if (!chan)
2814                 return -ENOENT;
2815
2816         sk = chan->sk;
2817
2818         if (chan->state != BT_CONFIG && chan->state != BT_CONNECT2) {
2819                 struct l2cap_cmd_rej_cid rej;
2820
2821                 rej.reason = cpu_to_le16(L2CAP_REJ_INVALID_CID);
2822                 rej.scid = cpu_to_le16(chan->scid);
2823                 rej.dcid = cpu_to_le16(chan->dcid);
2824
2825                 l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ,
2826                                 sizeof(rej), &rej);
2827                 goto unlock;
2828         }
2829
2830         /* Reject if config buffer is too small. */
2831         len = cmd_len - sizeof(*req);
2832         if (len < 0 || chan->conf_len + len > sizeof(chan->conf_req)) {
2833                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2834                                 l2cap_build_conf_rsp(chan, rsp,
2835                                         L2CAP_CONF_REJECT, flags), rsp);
2836                 goto unlock;
2837         }
2838
2839         /* Store config. */
2840         memcpy(chan->conf_req + chan->conf_len, req->data, len);
2841         chan->conf_len += len;
2842
2843         if (flags & 0x0001) {
2844                 /* Incomplete config. Send empty response. */
2845                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2846                                 l2cap_build_conf_rsp(chan, rsp,
2847                                         L2CAP_CONF_SUCCESS, 0x0001), rsp);
2848                 goto unlock;
2849         }
2850
2851         /* Complete config. */
2852         len = l2cap_parse_conf_req(chan, rsp);
2853         if (len < 0) {
2854                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
2855                 goto unlock;
2856         }
2857
2858         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
2859         chan->num_conf_rsp++;
2860
2861         /* Reset config buffer. */
2862         chan->conf_len = 0;
2863
2864         if (!test_bit(CONF_OUTPUT_DONE, &chan->conf_state))
2865                 goto unlock;
2866
2867         if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
2868                 set_default_fcs(chan);
2869
2870                 l2cap_state_change(chan, BT_CONNECTED);
2871
2872                 chan->next_tx_seq = 0;
2873                 chan->expected_tx_seq = 0;
2874                 skb_queue_head_init(&chan->tx_q);
2875                 if (chan->mode == L2CAP_MODE_ERTM)
2876                         l2cap_ertm_init(chan);
2877
2878                 l2cap_chan_ready(sk);
2879                 goto unlock;
2880         }
2881
2882         if (!test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) {
2883                 u8 buf[64];
2884                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2885                                         l2cap_build_conf_req(chan, buf), buf);
2886                 chan->num_conf_req++;
2887         }
2888
2889         /* Got Conf Rsp PENDING from remote side and asume we sent
2890            Conf Rsp PENDING in the code above */
2891         if (test_bit(CONF_REM_CONF_PEND, &chan->conf_state) &&
2892                         test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
2893
2894                 /* check compatibility */
2895
2896                 clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
2897                 set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
2898
2899                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2900                                         l2cap_build_conf_rsp(chan, rsp,
2901                                         L2CAP_CONF_SUCCESS, 0x0000), rsp);
2902         }
2903
2904 unlock:
2905         release_sock(sk);
2906         return 0;
2907 }
2908
2909 static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2910 {
2911         struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
2912         u16 scid, flags, result;
2913         struct l2cap_chan *chan;
2914         struct sock *sk;
2915         int len = cmd->len - sizeof(*rsp);
2916
2917         scid   = __le16_to_cpu(rsp->scid);
2918         flags  = __le16_to_cpu(rsp->flags);
2919         result = __le16_to_cpu(rsp->result);
2920
2921         BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x",
2922                         scid, flags, result);
2923
2924         chan = l2cap_get_chan_by_scid(conn, scid);
2925         if (!chan)
2926                 return 0;
2927
2928         sk = chan->sk;
2929
2930         switch (result) {
2931         case L2CAP_CONF_SUCCESS:
2932                 l2cap_conf_rfc_get(chan, rsp->data, len);
2933                 clear_bit(CONF_REM_CONF_PEND, &chan->conf_state);
2934                 break;
2935
2936         case L2CAP_CONF_PENDING:
2937                 set_bit(CONF_REM_CONF_PEND, &chan->conf_state);
2938
2939                 if (test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
2940                         char buf[64];
2941
2942                         len = l2cap_parse_conf_rsp(chan, rsp->data, len,
2943                                                                 buf, &result);
2944                         if (len < 0) {
2945                                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
2946                                 goto done;
2947                         }
2948
2949                         /* check compatibility */
2950
2951                         clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
2952                         set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
2953
2954                         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2955                                                 l2cap_build_conf_rsp(chan, buf,
2956                                                 L2CAP_CONF_SUCCESS, 0x0000), buf);
2957                 }
2958                 goto done;
2959
2960         case L2CAP_CONF_UNACCEPT:
2961                 if (chan->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
2962                         char req[64];
2963
2964                         if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) {
2965                                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
2966                                 goto done;
2967                         }
2968
2969                         /* throw out any old stored conf requests */
2970                         result = L2CAP_CONF_SUCCESS;
2971                         len = l2cap_parse_conf_rsp(chan, rsp->data, len,
2972                                                                 req, &result);
2973                         if (len < 0) {
2974                                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
2975                                 goto done;
2976                         }
2977
2978                         l2cap_send_cmd(conn, l2cap_get_ident(conn),
2979                                                 L2CAP_CONF_REQ, len, req);
2980                         chan->num_conf_req++;
2981                         if (result != L2CAP_CONF_SUCCESS)
2982                                 goto done;
2983                         break;
2984                 }
2985
2986         default:
2987                 sk->sk_err = ECONNRESET;
2988                 __set_chan_timer(chan, L2CAP_DISC_REJ_TIMEOUT);
2989                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
2990                 goto done;
2991         }
2992
2993         if (flags & 0x01)
2994                 goto done;
2995
2996         set_bit(CONF_INPUT_DONE, &chan->conf_state);
2997
2998         if (test_bit(CONF_OUTPUT_DONE, &chan->conf_state)) {
2999                 set_default_fcs(chan);
3000
3001                 l2cap_state_change(chan, BT_CONNECTED);
3002                 chan->next_tx_seq = 0;
3003                 chan->expected_tx_seq = 0;
3004                 skb_queue_head_init(&chan->tx_q);
3005                 if (chan->mode ==  L2CAP_MODE_ERTM)
3006                         l2cap_ertm_init(chan);
3007
3008                 l2cap_chan_ready(sk);
3009         }
3010
3011 done:
3012         release_sock(sk);
3013         return 0;
3014 }
3015
3016 static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3017 {
3018         struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
3019         struct l2cap_disconn_rsp rsp;
3020         u16 dcid, scid;
3021         struct l2cap_chan *chan;
3022         struct sock *sk;
3023
3024         scid = __le16_to_cpu(req->scid);
3025         dcid = __le16_to_cpu(req->dcid);
3026
3027         BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
3028
3029         chan = l2cap_get_chan_by_scid(conn, dcid);
3030         if (!chan)
3031                 return 0;
3032
3033         sk = chan->sk;
3034
3035         rsp.dcid = cpu_to_le16(chan->scid);
3036         rsp.scid = cpu_to_le16(chan->dcid);
3037         l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
3038
3039         sk->sk_shutdown = SHUTDOWN_MASK;
3040
3041         l2cap_chan_del(chan, ECONNRESET);
3042         release_sock(sk);
3043
3044         chan->ops->close(chan->data);
3045         return 0;
3046 }
3047
3048 static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3049 {
3050         struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
3051         u16 dcid, scid;
3052         struct l2cap_chan *chan;
3053         struct sock *sk;
3054
3055         scid = __le16_to_cpu(rsp->scid);
3056         dcid = __le16_to_cpu(rsp->dcid);
3057
3058         BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
3059
3060         chan = l2cap_get_chan_by_scid(conn, scid);
3061         if (!chan)
3062                 return 0;
3063
3064         sk = chan->sk;
3065
3066         l2cap_chan_del(chan, 0);
3067         release_sock(sk);
3068
3069         chan->ops->close(chan->data);
3070         return 0;
3071 }
3072
3073 static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3074 {
3075         struct l2cap_info_req *req = (struct l2cap_info_req *) data;
3076         u16 type;
3077
3078         type = __le16_to_cpu(req->type);
3079
3080         BT_DBG("type 0x%4.4x", type);
3081
3082         if (type == L2CAP_IT_FEAT_MASK) {
3083                 u8 buf[8];
3084                 u32 feat_mask = l2cap_feat_mask;
3085                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
3086                 rsp->type   = cpu_to_le16(L2CAP_IT_FEAT_MASK);
3087                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
3088                 if (!disable_ertm)
3089                         feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
3090                                                          | L2CAP_FEAT_FCS;
3091                 if (enable_hs)
3092                         feat_mask |= L2CAP_FEAT_EXT_FLOW
3093                                                 | L2CAP_FEAT_EXT_WINDOW;
3094
3095                 put_unaligned_le32(feat_mask, rsp->data);
3096                 l2cap_send_cmd(conn, cmd->ident,
3097                                         L2CAP_INFO_RSP, sizeof(buf), buf);
3098         } else if (type == L2CAP_IT_FIXED_CHAN) {
3099                 u8 buf[12];
3100                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
3101
3102                 if (enable_hs)
3103                         l2cap_fixed_chan[0] |= L2CAP_FC_A2MP;
3104                 else
3105                         l2cap_fixed_chan[0] &= ~L2CAP_FC_A2MP;
3106
3107                 rsp->type   = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
3108                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
3109                 memcpy(rsp->data, l2cap_fixed_chan, sizeof(l2cap_fixed_chan));
3110                 l2cap_send_cmd(conn, cmd->ident,
3111                                         L2CAP_INFO_RSP, sizeof(buf), buf);
3112         } else {
3113                 struct l2cap_info_rsp rsp;
3114                 rsp.type   = cpu_to_le16(type);
3115                 rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP);
3116                 l2cap_send_cmd(conn, cmd->ident,
3117                                         L2CAP_INFO_RSP, sizeof(rsp), &rsp);
3118         }
3119
3120         return 0;
3121 }
3122
3123 static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3124 {
3125         struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
3126         u16 type, result;
3127
3128         type   = __le16_to_cpu(rsp->type);
3129         result = __le16_to_cpu(rsp->result);
3130
3131         BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
3132
3133         /* L2CAP Info req/rsp are unbound to channels, add extra checks */
3134         if (cmd->ident != conn->info_ident ||
3135                         conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)
3136                 return 0;
3137
3138         cancel_delayed_work_sync(&conn->info_work);
3139
3140         if (result != L2CAP_IR_SUCCESS) {
3141                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3142                 conn->info_ident = 0;
3143
3144                 l2cap_conn_start(conn);
3145
3146                 return 0;
3147         }
3148
3149         if (type == L2CAP_IT_FEAT_MASK) {
3150                 conn->feat_mask = get_unaligned_le32(rsp->data);
3151
3152                 if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
3153                         struct l2cap_info_req req;
3154                         req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
3155
3156                         conn->info_ident = l2cap_get_ident(conn);
3157
3158                         l2cap_send_cmd(conn, conn->info_ident,
3159                                         L2CAP_INFO_REQ, sizeof(req), &req);
3160                 } else {
3161                         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3162                         conn->info_ident = 0;
3163
3164                         l2cap_conn_start(conn);
3165                 }
3166         } else if (type == L2CAP_IT_FIXED_CHAN) {
3167                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3168                 conn->info_ident = 0;
3169
3170                 l2cap_conn_start(conn);
3171         }
3172
3173         return 0;
3174 }
3175
3176 static inline int l2cap_create_channel_req(struct l2cap_conn *conn,
3177                                         struct l2cap_cmd_hdr *cmd, u16 cmd_len,
3178                                         void *data)
3179 {
3180         struct l2cap_create_chan_req *req = data;
3181         struct l2cap_create_chan_rsp rsp;
3182         u16 psm, scid;
3183
3184         if (cmd_len != sizeof(*req))
3185                 return -EPROTO;
3186
3187         if (!enable_hs)
3188                 return -EINVAL;
3189
3190         psm = le16_to_cpu(req->psm);
3191         scid = le16_to_cpu(req->scid);
3192
3193         BT_DBG("psm %d, scid %d, amp_id %d", psm, scid, req->amp_id);
3194
3195         /* Placeholder: Always reject */
3196         rsp.dcid = 0;
3197         rsp.scid = cpu_to_le16(scid);
3198         rsp.result = L2CAP_CR_NO_MEM;
3199         rsp.status = L2CAP_CS_NO_INFO;
3200
3201         l2cap_send_cmd(conn, cmd->ident, L2CAP_CREATE_CHAN_RSP,
3202                        sizeof(rsp), &rsp);
3203
3204         return 0;
3205 }
3206
3207 static inline int l2cap_create_channel_rsp(struct l2cap_conn *conn,
3208                                         struct l2cap_cmd_hdr *cmd, void *data)
3209 {
3210         BT_DBG("conn %p", conn);
3211
3212         return l2cap_connect_rsp(conn, cmd, data);
3213 }
3214
3215 static void l2cap_send_move_chan_rsp(struct l2cap_conn *conn, u8 ident,
3216                                                         u16 icid, u16 result)
3217 {
3218         struct l2cap_move_chan_rsp rsp;
3219
3220         BT_DBG("icid %d, result %d", icid, result);
3221
3222         rsp.icid = cpu_to_le16(icid);
3223         rsp.result = cpu_to_le16(result);
3224
3225         l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_RSP, sizeof(rsp), &rsp);
3226 }
3227
3228 static void l2cap_send_move_chan_cfm(struct l2cap_conn *conn,
3229                                 struct l2cap_chan *chan, u16 icid, u16 result)
3230 {
3231         struct l2cap_move_chan_cfm cfm;
3232         u8 ident;
3233
3234         BT_DBG("icid %d, result %d", icid, result);
3235
3236         ident = l2cap_get_ident(conn);
3237         if (chan)
3238                 chan->ident = ident;
3239
3240         cfm.icid = cpu_to_le16(icid);
3241         cfm.result = cpu_to_le16(result);
3242
3243         l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM, sizeof(cfm), &cfm);
3244 }
3245
3246 static void l2cap_send_move_chan_cfm_rsp(struct l2cap_conn *conn, u8 ident,
3247                                                                 u16 icid)
3248 {
3249         struct l2cap_move_chan_cfm_rsp rsp;
3250
3251         BT_DBG("icid %d", icid);
3252
3253         rsp.icid = cpu_to_le16(icid);
3254         l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM_RSP, sizeof(rsp), &rsp);
3255 }
3256
3257 static inline int l2cap_move_channel_req(struct l2cap_conn *conn,
3258                         struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data)
3259 {
3260         struct l2cap_move_chan_req *req = data;
3261         u16 icid = 0;
3262         u16 result = L2CAP_MR_NOT_ALLOWED;
3263
3264         if (cmd_len != sizeof(*req))
3265                 return -EPROTO;
3266
3267         icid = le16_to_cpu(req->icid);
3268
3269         BT_DBG("icid %d, dest_amp_id %d", icid, req->dest_amp_id);
3270
3271         if (!enable_hs)
3272                 return -EINVAL;
3273
3274         /* Placeholder: Always refuse */
3275         l2cap_send_move_chan_rsp(conn, cmd->ident, icid, result);
3276
3277         return 0;
3278 }
3279
3280 static inline int l2cap_move_channel_rsp(struct l2cap_conn *conn,
3281                         struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data)
3282 {
3283         struct l2cap_move_chan_rsp *rsp = data;
3284         u16 icid, result;
3285
3286         if (cmd_len != sizeof(*rsp))
3287                 return -EPROTO;
3288
3289         icid = le16_to_cpu(rsp->icid);
3290         result = le16_to_cpu(rsp->result);
3291
3292         BT_DBG("icid %d, result %d", icid, result);
3293
3294         /* Placeholder: Always unconfirmed */
3295         l2cap_send_move_chan_cfm(conn, NULL, icid, L2CAP_MC_UNCONFIRMED);
3296
3297         return 0;
3298 }
3299
3300 static inline int l2cap_move_channel_confirm(struct l2cap_conn *conn,
3301                         struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data)
3302 {
3303         struct l2cap_move_chan_cfm *cfm = data;
3304         u16 icid, result;
3305
3306         if (cmd_len != sizeof(*cfm))
3307                 return -EPROTO;
3308
3309         icid = le16_to_cpu(cfm->icid);
3310         result = le16_to_cpu(cfm->result);
3311
3312         BT_DBG("icid %d, result %d", icid, result);
3313
3314         l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid);
3315
3316         return 0;
3317 }
3318
3319 static inline int l2cap_move_channel_confirm_rsp(struct l2cap_conn *conn,
3320                         struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data)
3321 {
3322         struct l2cap_move_chan_cfm_rsp *rsp = data;
3323         u16 icid;
3324
3325         if (cmd_len != sizeof(*rsp))
3326                 return -EPROTO;
3327
3328         icid = le16_to_cpu(rsp->icid);
3329
3330         BT_DBG("icid %d", icid);
3331
3332         return 0;
3333 }
3334
3335 static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency,
3336                                                         u16 to_multiplier)
3337 {
3338         u16 max_latency;
3339
3340         if (min > max || min < 6 || max > 3200)
3341                 return -EINVAL;
3342
3343         if (to_multiplier < 10 || to_multiplier > 3200)
3344                 return -EINVAL;
3345
3346         if (max >= to_multiplier * 8)
3347                 return -EINVAL;
3348
3349         max_latency = (to_multiplier * 8 / max) - 1;
3350         if (latency > 499 || latency > max_latency)
3351                 return -EINVAL;
3352
3353         return 0;
3354 }
3355
3356 static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
3357                                         struct l2cap_cmd_hdr *cmd, u8 *data)
3358 {
3359         struct hci_conn *hcon = conn->hcon;
3360         struct l2cap_conn_param_update_req *req;
3361         struct l2cap_conn_param_update_rsp rsp;
3362         u16 min, max, latency, to_multiplier, cmd_len;
3363         int err;
3364
3365         if (!(hcon->link_mode & HCI_LM_MASTER))
3366                 return -EINVAL;
3367
3368         cmd_len = __le16_to_cpu(cmd->len);
3369         if (cmd_len != sizeof(struct l2cap_conn_param_update_req))
3370                 return -EPROTO;
3371
3372         req = (struct l2cap_conn_param_update_req *) data;
3373         min             = __le16_to_cpu(req->min);
3374         max             = __le16_to_cpu(req->max);
3375         latency         = __le16_to_cpu(req->latency);
3376         to_multiplier   = __le16_to_cpu(req->to_multiplier);
3377
3378         BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x",
3379                                                 min, max, latency, to_multiplier);
3380
3381         memset(&rsp, 0, sizeof(rsp));
3382
3383         err = l2cap_check_conn_param(min, max, latency, to_multiplier);
3384         if (err)
3385                 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_REJECTED);
3386         else
3387                 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED);
3388
3389         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP,
3390                                                         sizeof(rsp), &rsp);
3391
3392         if (!err)
3393                 hci_le_conn_update(hcon, min, max, latency, to_multiplier);
3394
3395         return 0;
3396 }
3397
3398 static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
3399                         struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
3400 {
3401         int err = 0;
3402
3403         switch (cmd->code) {
3404         case L2CAP_COMMAND_REJ:
3405                 l2cap_command_rej(conn, cmd, data);
3406                 break;
3407
3408         case L2CAP_CONN_REQ:
3409                 err = l2cap_connect_req(conn, cmd, data);
3410                 break;
3411
3412         case L2CAP_CONN_RSP:
3413                 err = l2cap_connect_rsp(conn, cmd, data);
3414                 break;
3415
3416         case L2CAP_CONF_REQ:
3417                 err = l2cap_config_req(conn, cmd, cmd_len, data);
3418                 break;
3419
3420         case L2CAP_CONF_RSP:
3421                 err = l2cap_config_rsp(conn, cmd, data);
3422                 break;
3423
3424         case L2CAP_DISCONN_REQ:
3425                 err = l2cap_disconnect_req(conn, cmd, data);
3426                 break;
3427
3428         case L2CAP_DISCONN_RSP:
3429                 err = l2cap_disconnect_rsp(conn, cmd, data);
3430                 break;
3431
3432         case L2CAP_ECHO_REQ:
3433                 l2cap_send_cmd(conn, cmd->ident, L2CAP_ECHO_RSP, cmd_len, data);
3434                 break;
3435
3436         case L2CAP_ECHO_RSP:
3437                 break;
3438
3439         case L2CAP_INFO_REQ:
3440                 err = l2cap_information_req(conn, cmd, data);
3441                 break;
3442
3443         case L2CAP_INFO_RSP:
3444                 err = l2cap_information_rsp(conn, cmd, data);
3445                 break;
3446
3447         case L2CAP_CREATE_CHAN_REQ:
3448                 err = l2cap_create_channel_req(conn, cmd, cmd_len, data);
3449                 break;
3450
3451         case L2CAP_CREATE_CHAN_RSP:
3452                 err = l2cap_create_channel_rsp(conn, cmd, data);
3453                 break;
3454
3455         case L2CAP_MOVE_CHAN_REQ:
3456                 err = l2cap_move_channel_req(conn, cmd, cmd_len, data);
3457                 break;
3458
3459         case L2CAP_MOVE_CHAN_RSP:
3460                 err = l2cap_move_channel_rsp(conn, cmd, cmd_len, data);
3461                 break;
3462
3463         case L2CAP_MOVE_CHAN_CFM:
3464                 err = l2cap_move_channel_confirm(conn, cmd, cmd_len, data);
3465                 break;
3466
3467         case L2CAP_MOVE_CHAN_CFM_RSP:
3468                 err = l2cap_move_channel_confirm_rsp(conn, cmd, cmd_len, data);
3469                 break;
3470
3471         default:
3472                 BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code);
3473                 err = -EINVAL;
3474                 break;
3475         }
3476
3477         return err;
3478 }
3479
3480 static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn,
3481                                         struct l2cap_cmd_hdr *cmd, u8 *data)
3482 {
3483         switch (cmd->code) {
3484         case L2CAP_COMMAND_REJ:
3485                 return 0;
3486
3487         case L2CAP_CONN_PARAM_UPDATE_REQ:
3488                 return l2cap_conn_param_update_req(conn, cmd, data);
3489
3490         case L2CAP_CONN_PARAM_UPDATE_RSP:
3491                 return 0;
3492
3493         default:
3494                 BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code);
3495                 return -EINVAL;
3496         }
3497 }
3498
3499 static inline void l2cap_sig_channel(struct l2cap_conn *conn,
3500                                                         struct sk_buff *skb)
3501 {
3502         u8 *data = skb->data;
3503         int len = skb->len;
3504         struct l2cap_cmd_hdr cmd;
3505         int err;
3506
3507         l2cap_raw_recv(conn, skb);
3508
3509         while (len >= L2CAP_CMD_HDR_SIZE) {
3510                 u16 cmd_len;
3511                 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
3512                 data += L2CAP_CMD_HDR_SIZE;
3513                 len  -= L2CAP_CMD_HDR_SIZE;
3514
3515                 cmd_len = le16_to_cpu(cmd.len);
3516
3517                 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len, cmd.ident);
3518
3519                 if (cmd_len > len || !cmd.ident) {
3520                         BT_DBG("corrupted command");
3521                         break;
3522                 }
3523
3524                 if (conn->hcon->type == LE_LINK)
3525                         err = l2cap_le_sig_cmd(conn, &cmd, data);
3526                 else
3527                         err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data);
3528
3529                 if (err) {
3530                         struct l2cap_cmd_rej_unk rej;
3531
3532                         BT_ERR("Wrong link type (%d)", err);
3533
3534                         /* FIXME: Map err to a valid reason */
3535                         rej.reason = cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
3536                         l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
3537                 }
3538
3539                 data += cmd_len;
3540                 len  -= cmd_len;
3541         }
3542
3543         kfree_skb(skb);
3544 }
3545
3546 static int l2cap_check_fcs(struct l2cap_chan *chan,  struct sk_buff *skb)
3547 {
3548         u16 our_fcs, rcv_fcs;
3549         int hdr_size;
3550
3551         if (test_bit(FLAG_EXT_CTRL, &chan->flags))
3552                 hdr_size = L2CAP_EXT_HDR_SIZE;
3553         else
3554                 hdr_size = L2CAP_ENH_HDR_SIZE;
3555
3556         if (chan->fcs == L2CAP_FCS_CRC16) {
3557                 skb_trim(skb, skb->len - L2CAP_FCS_SIZE);
3558                 rcv_fcs = get_unaligned_le16(skb->data + skb->len);
3559                 our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
3560
3561                 if (our_fcs != rcv_fcs)
3562                         return -EBADMSG;
3563         }
3564         return 0;
3565 }
3566
3567 static inline void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan)
3568 {
3569         u32 control = 0;
3570
3571         chan->frames_sent = 0;
3572
3573         control |= __set_reqseq(chan, chan->buffer_seq);
3574
3575         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
3576                 control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR);
3577                 l2cap_send_sframe(chan, control);
3578                 set_bit(CONN_RNR_SENT, &chan->conn_state);
3579         }
3580
3581         if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
3582                 l2cap_retransmit_frames(chan);
3583
3584         l2cap_ertm_send(chan);
3585
3586         if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state) &&
3587                         chan->frames_sent == 0) {
3588                 control |= __set_ctrl_super(chan, L2CAP_SUPER_RR);
3589                 l2cap_send_sframe(chan, control);
3590         }
3591 }
3592
3593 static int l2cap_add_to_srej_queue(struct l2cap_chan *chan, struct sk_buff *skb, u16 tx_seq, u8 sar)
3594 {
3595         struct sk_buff *next_skb;
3596         int tx_seq_offset, next_tx_seq_offset;
3597
3598         bt_cb(skb)->tx_seq = tx_seq;
3599         bt_cb(skb)->sar = sar;
3600
3601         next_skb = skb_peek(&chan->srej_q);
3602
3603         tx_seq_offset = __seq_offset(chan, tx_seq, chan->buffer_seq);
3604
3605         while (next_skb) {
3606                 if (bt_cb(next_skb)->tx_seq == tx_seq)
3607                         return -EINVAL;
3608
3609                 next_tx_seq_offset = __seq_offset(chan,
3610                                 bt_cb(next_skb)->tx_seq, chan->buffer_seq);
3611
3612                 if (next_tx_seq_offset > tx_seq_offset) {
3613                         __skb_queue_before(&chan->srej_q, next_skb, skb);
3614                         return 0;
3615                 }
3616
3617                 if (skb_queue_is_last(&chan->srej_q, next_skb))
3618                         next_skb = NULL;
3619                 else
3620                         next_skb = skb_queue_next(&chan->srej_q, next_skb);
3621         }
3622
3623         __skb_queue_tail(&chan->srej_q, skb);
3624
3625         return 0;
3626 }
3627
3628 static void append_skb_frag(struct sk_buff *skb,
3629                         struct sk_buff *new_frag, struct sk_buff **last_frag)
3630 {
3631         /* skb->len reflects data in skb as well as all fragments
3632          * skb->data_len reflects only data in fragments
3633          */
3634         if (!skb_has_frag_list(skb))
3635                 skb_shinfo(skb)->frag_list = new_frag;
3636
3637         new_frag->next = NULL;
3638
3639         (*last_frag)->next = new_frag;
3640         *last_frag = new_frag;
3641
3642         skb->len += new_frag->len;
3643         skb->data_len += new_frag->len;
3644         skb->truesize += new_frag->truesize;
3645 }
3646
3647 static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u32 control)
3648 {
3649         int err = -EINVAL;
3650
3651         switch (__get_ctrl_sar(chan, control)) {
3652         case L2CAP_SAR_UNSEGMENTED:
3653                 if (chan->sdu)
3654                         break;
3655
3656                 err = chan->ops->recv(chan->data, skb);
3657                 break;
3658
3659         case L2CAP_SAR_START:
3660                 if (chan->sdu)
3661                         break;
3662
3663                 chan->sdu_len = get_unaligned_le16(skb->data);
3664                 skb_pull(skb, L2CAP_SDULEN_SIZE);
3665
3666                 if (chan->sdu_len > chan->imtu) {
3667                         err = -EMSGSIZE;
3668                         break;
3669                 }
3670
3671                 if (skb->len >= chan->sdu_len)
3672                         break;
3673
3674                 chan->sdu = skb;
3675                 chan->sdu_last_frag = skb;
3676
3677                 skb = NULL;
3678                 err = 0;
3679                 break;
3680
3681         case L2CAP_SAR_CONTINUE:
3682                 if (!chan->sdu)
3683                         break;
3684
3685                 append_skb_frag(chan->sdu, skb,
3686                                 &chan->sdu_last_frag);
3687                 skb = NULL;
3688
3689                 if (chan->sdu->len >= chan->sdu_len)
3690                         break;
3691
3692                 err = 0;
3693                 break;
3694
3695         case L2CAP_SAR_END:
3696                 if (!chan->sdu)
3697                         break;
3698
3699                 append_skb_frag(chan->sdu, skb,
3700                                 &chan->sdu_last_frag);
3701                 skb = NULL;
3702
3703                 if (chan->sdu->len != chan->sdu_len)
3704                         break;
3705
3706                 err = chan->ops->recv(chan->data, chan->sdu);
3707
3708                 if (!err) {
3709                         /* Reassembly complete */
3710                         chan->sdu = NULL;
3711                         chan->sdu_last_frag = NULL;
3712                         chan->sdu_len = 0;
3713                 }
3714                 break;
3715         }
3716
3717         if (err) {
3718                 kfree_skb(skb);
3719                 kfree_skb(chan->sdu);
3720                 chan->sdu = NULL;
3721                 chan->sdu_last_frag = NULL;
3722                 chan->sdu_len = 0;
3723         }
3724
3725         return err;
3726 }
3727
3728 static void l2cap_ertm_enter_local_busy(struct l2cap_chan *chan)
3729 {
3730         u32 control;
3731
3732         BT_DBG("chan %p, Enter local busy", chan);
3733
3734         set_bit(CONN_LOCAL_BUSY, &chan->conn_state);
3735
3736         control = __set_reqseq(chan, chan->buffer_seq);
3737         control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR);
3738         l2cap_send_sframe(chan, control);
3739
3740         set_bit(CONN_RNR_SENT, &chan->conn_state);
3741
3742         __clear_ack_timer(chan);
3743 }
3744
3745 static void l2cap_ertm_exit_local_busy(struct l2cap_chan *chan)
3746 {
3747         u32 control;
3748
3749         if (!test_bit(CONN_RNR_SENT, &chan->conn_state))
3750                 goto done;
3751
3752         control = __set_reqseq(chan, chan->buffer_seq);
3753         control |= __set_ctrl_poll(chan);
3754         control |= __set_ctrl_super(chan, L2CAP_SUPER_RR);
3755         l2cap_send_sframe(chan, control);
3756         chan->retry_count = 1;
3757
3758         __clear_retrans_timer(chan);
3759         __set_monitor_timer(chan);
3760
3761         set_bit(CONN_WAIT_F, &chan->conn_state);
3762
3763 done:
3764         clear_bit(CONN_LOCAL_BUSY, &chan->conn_state);
3765         clear_bit(CONN_RNR_SENT, &chan->conn_state);
3766
3767         BT_DBG("chan %p, Exit local busy", chan);
3768 }
3769
3770 void l2cap_chan_busy(struct l2cap_chan *chan, int busy)
3771 {
3772         if (chan->mode == L2CAP_MODE_ERTM) {
3773                 if (busy)
3774                         l2cap_ertm_enter_local_busy(chan);
3775                 else
3776                         l2cap_ertm_exit_local_busy(chan);
3777         }
3778 }
3779
3780 static void l2cap_check_srej_gap(struct l2cap_chan *chan, u16 tx_seq)
3781 {
3782         struct sk_buff *skb;
3783         u32 control;
3784
3785         while ((skb = skb_peek(&chan->srej_q)) &&
3786                         !test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
3787                 int err;
3788
3789                 if (bt_cb(skb)->tx_seq != tx_seq)
3790                         break;
3791
3792                 skb = skb_dequeue(&chan->srej_q);
3793                 control = __set_ctrl_sar(chan, bt_cb(skb)->sar);
3794                 err = l2cap_reassemble_sdu(chan, skb, control);
3795
3796                 if (err < 0) {
3797                         l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3798                         break;
3799                 }
3800
3801                 chan->buffer_seq_srej = __next_seq(chan, chan->buffer_seq_srej);
3802                 tx_seq = __next_seq(chan, tx_seq);
3803         }
3804 }
3805
3806 static void l2cap_resend_srejframe(struct l2cap_chan *chan, u16 tx_seq)
3807 {
3808         struct srej_list *l, *tmp;
3809         u32 control;
3810
3811         list_for_each_entry_safe(l, tmp, &chan->srej_l, list) {
3812                 if (l->tx_seq == tx_seq) {
3813                         list_del(&l->list);
3814                         kfree(l);
3815                         return;
3816                 }
3817                 control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ);
3818                 control |= __set_reqseq(chan, l->tx_seq);
3819                 l2cap_send_sframe(chan, control);
3820                 list_del(&l->list);
3821                 list_add_tail(&l->list, &chan->srej_l);
3822         }
3823 }
3824
3825 static int l2cap_send_srejframe(struct l2cap_chan *chan, u16 tx_seq)
3826 {
3827         struct srej_list *new;
3828         u32 control;
3829
3830         while (tx_seq != chan->expected_tx_seq) {
3831                 control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ);
3832                 control |= __set_reqseq(chan, chan->expected_tx_seq);
3833                 l2cap_send_sframe(chan, control);
3834
3835                 new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC);
3836                 if (!new)
3837                         return -ENOMEM;
3838
3839                 new->tx_seq = chan->expected_tx_seq;
3840
3841                 chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq);
3842
3843                 list_add_tail(&new->list, &chan->srej_l);
3844         }
3845
3846         chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq);
3847
3848         return 0;
3849 }
3850
3851 static inline int l2cap_data_channel_iframe(struct l2cap_chan *chan, u32 rx_control, struct sk_buff *skb)
3852 {
3853         u16 tx_seq = __get_txseq(chan, rx_control);
3854         u16 req_seq = __get_reqseq(chan, rx_control);
3855         u8 sar = __get_ctrl_sar(chan, rx_control);
3856         int tx_seq_offset, expected_tx_seq_offset;
3857         int num_to_ack = (chan->tx_win/6) + 1;
3858         int err = 0;
3859
3860         BT_DBG("chan %p len %d tx_seq %d rx_control 0x%8.8x", chan, skb->len,
3861                                                         tx_seq, rx_control);
3862
3863         if (__is_ctrl_final(chan, rx_control) &&
3864                         test_bit(CONN_WAIT_F, &chan->conn_state)) {
3865                 __clear_monitor_timer(chan);
3866                 if (chan->unacked_frames > 0)
3867                         __set_retrans_timer(chan);
3868                 clear_bit(CONN_WAIT_F, &chan->conn_state);
3869         }
3870
3871         chan->expected_ack_seq = req_seq;
3872         l2cap_drop_acked_frames(chan);
3873
3874         tx_seq_offset = __seq_offset(chan, tx_seq, chan->buffer_seq);
3875
3876         /* invalid tx_seq */
3877         if (tx_seq_offset >= chan->tx_win) {
3878                 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3879                 goto drop;
3880         }
3881
3882         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state))
3883                 goto drop;
3884
3885         if (tx_seq == chan->expected_tx_seq)
3886                 goto expected;
3887
3888         if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
3889                 struct srej_list *first;
3890
3891                 first = list_first_entry(&chan->srej_l,
3892                                 struct srej_list, list);
3893                 if (tx_seq == first->tx_seq) {
3894                         l2cap_add_to_srej_queue(chan, skb, tx_seq, sar);
3895                         l2cap_check_srej_gap(chan, tx_seq);
3896
3897                         list_del(&first->list);
3898                         kfree(first);
3899
3900                         if (list_empty(&chan->srej_l)) {
3901                                 chan->buffer_seq = chan->buffer_seq_srej;
3902                                 clear_bit(CONN_SREJ_SENT, &chan->conn_state);
3903                                 l2cap_send_ack(chan);
3904                                 BT_DBG("chan %p, Exit SREJ_SENT", chan);
3905                         }
3906                 } else {
3907                         struct srej_list *l;
3908
3909                         /* duplicated tx_seq */
3910                         if (l2cap_add_to_srej_queue(chan, skb, tx_seq, sar) < 0)
3911                                 goto drop;
3912
3913                         list_for_each_entry(l, &chan->srej_l, list) {
3914                                 if (l->tx_seq == tx_seq) {
3915                                         l2cap_resend_srejframe(chan, tx_seq);
3916                                         return 0;
3917                                 }
3918                         }
3919
3920                         err = l2cap_send_srejframe(chan, tx_seq);
3921                         if (err < 0) {
3922                                 l2cap_send_disconn_req(chan->conn, chan, -err);
3923                                 return err;
3924                         }
3925                 }
3926         } else {
3927                 expected_tx_seq_offset = __seq_offset(chan,
3928                                 chan->expected_tx_seq, chan->buffer_seq);
3929
3930                 /* duplicated tx_seq */
3931                 if (tx_seq_offset < expected_tx_seq_offset)
3932                         goto drop;
3933
3934                 set_bit(CONN_SREJ_SENT, &chan->conn_state);
3935
3936                 BT_DBG("chan %p, Enter SREJ", chan);
3937
3938                 INIT_LIST_HEAD(&chan->srej_l);
3939                 chan->buffer_seq_srej = chan->buffer_seq;
3940
3941                 __skb_queue_head_init(&chan->srej_q);
3942                 l2cap_add_to_srej_queue(chan, skb, tx_seq, sar);
3943
3944                 set_bit(CONN_SEND_PBIT, &chan->conn_state);
3945
3946                 err = l2cap_send_srejframe(chan, tx_seq);
3947                 if (err < 0) {
3948                         l2cap_send_disconn_req(chan->conn, chan, -err);
3949                         return err;
3950                 }
3951
3952                 __clear_ack_timer(chan);
3953         }
3954         return 0;
3955
3956 expected:
3957         chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq);
3958
3959         if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
3960                 bt_cb(skb)->tx_seq = tx_seq;
3961                 bt_cb(skb)->sar = sar;
3962                 __skb_queue_tail(&chan->srej_q, skb);
3963                 return 0;
3964         }
3965
3966         err = l2cap_reassemble_sdu(chan, skb, rx_control);
3967         chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
3968
3969         if (err < 0) {
3970                 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3971                 return err;
3972         }
3973
3974         if (__is_ctrl_final(chan, rx_control)) {
3975                 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
3976                         l2cap_retransmit_frames(chan);
3977         }
3978
3979
3980         chan->num_acked = (chan->num_acked + 1) % num_to_ack;
3981         if (chan->num_acked == num_to_ack - 1)
3982                 l2cap_send_ack(chan);
3983         else
3984                 __set_ack_timer(chan);
3985
3986         return 0;
3987
3988 drop:
3989         kfree_skb(skb);
3990         return 0;
3991 }
3992
3993 static inline void l2cap_data_channel_rrframe(struct l2cap_chan *chan, u32 rx_control)
3994 {
3995         BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan,
3996                                 __get_reqseq(chan, rx_control), rx_control);
3997
3998         chan->expected_ack_seq = __get_reqseq(chan, rx_control);
3999         l2cap_drop_acked_frames(chan);
4000
4001         if (__is_ctrl_poll(chan, rx_control)) {
4002                 set_bit(CONN_SEND_FBIT, &chan->conn_state);
4003                 if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
4004                         if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
4005                                         (chan->unacked_frames > 0))
4006                                 __set_retrans_timer(chan);
4007
4008                         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
4009                         l2cap_send_srejtail(chan);
4010                 } else {
4011                         l2cap_send_i_or_rr_or_rnr(chan);
4012                 }
4013
4014         } else if (__is_ctrl_final(chan, rx_control)) {
4015                 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
4016
4017                 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
4018                         l2cap_retransmit_frames(chan);
4019
4020         } else {
4021                 if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
4022                                 (chan->unacked_frames > 0))
4023                         __set_retrans_timer(chan);
4024
4025                 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
4026                 if (test_bit(CONN_SREJ_SENT, &chan->conn_state))
4027                         l2cap_send_ack(chan);
4028                 else
4029                         l2cap_ertm_send(chan);
4030         }
4031 }
4032
4033 static inline void l2cap_data_channel_rejframe(struct l2cap_chan *chan, u32 rx_control)
4034 {
4035         u16 tx_seq = __get_reqseq(chan, rx_control);
4036
4037         BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control);
4038
4039         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
4040
4041         chan->expected_ack_seq = tx_seq;
4042         l2cap_drop_acked_frames(chan);
4043
4044         if (__is_ctrl_final(chan, rx_control)) {
4045                 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
4046                         l2cap_retransmit_frames(chan);
4047         } else {
4048                 l2cap_retransmit_frames(chan);
4049
4050                 if (test_bit(CONN_WAIT_F, &chan->conn_state))
4051                         set_bit(CONN_REJ_ACT, &chan->conn_state);
4052         }
4053 }
4054 static inline void l2cap_data_channel_srejframe(struct l2cap_chan *chan, u32 rx_control)
4055 {
4056         u16 tx_seq = __get_reqseq(chan, rx_control);
4057
4058         BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control);
4059
4060         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
4061
4062         if (__is_ctrl_poll(chan, rx_control)) {
4063                 chan->expected_ack_seq = tx_seq;
4064                 l2cap_drop_acked_frames(chan);
4065
4066                 set_bit(CONN_SEND_FBIT, &chan->conn_state);
4067                 l2cap_retransmit_one_frame(chan, tx_seq);
4068
4069                 l2cap_ertm_send(chan);
4070
4071                 if (test_bit(CONN_WAIT_F, &chan->conn_state)) {
4072                         chan->srej_save_reqseq = tx_seq;
4073                         set_bit(CONN_SREJ_ACT, &chan->conn_state);
4074                 }
4075         } else if (__is_ctrl_final(chan, rx_control)) {
4076                 if (test_bit(CONN_SREJ_ACT, &chan->conn_state) &&
4077                                 chan->srej_save_reqseq == tx_seq)
4078                         clear_bit(CONN_SREJ_ACT, &chan->conn_state);
4079                 else
4080                         l2cap_retransmit_one_frame(chan, tx_seq);
4081         } else {
4082                 l2cap_retransmit_one_frame(chan, tx_seq);
4083                 if (test_bit(CONN_WAIT_F, &chan->conn_state)) {
4084                         chan->srej_save_reqseq = tx_seq;
4085                         set_bit(CONN_SREJ_ACT, &chan->conn_state);
4086                 }
4087         }
4088 }
4089
4090 static inline void l2cap_data_channel_rnrframe(struct l2cap_chan *chan, u32 rx_control)
4091 {
4092         u16 tx_seq = __get_reqseq(chan, rx_control);
4093
4094         BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control);
4095
4096         set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
4097         chan->expected_ack_seq = tx_seq;
4098         l2cap_drop_acked_frames(chan);
4099
4100         if (__is_ctrl_poll(chan, rx_control))
4101                 set_bit(CONN_SEND_FBIT, &chan->conn_state);
4102
4103         if (!test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
4104                 __clear_retrans_timer(chan);
4105                 if (__is_ctrl_poll(chan, rx_control))
4106                         l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_FINAL);
4107                 return;
4108         }
4109
4110         if (__is_ctrl_poll(chan, rx_control)) {
4111                 l2cap_send_srejtail(chan);
4112         } else {
4113                 rx_control = __set_ctrl_super(chan, L2CAP_SUPER_RR);
4114                 l2cap_send_sframe(chan, rx_control);
4115         }
4116 }
4117
4118 static inline int l2cap_data_channel_sframe(struct l2cap_chan *chan, u32 rx_control, struct sk_buff *skb)
4119 {
4120         BT_DBG("chan %p rx_control 0x%8.8x len %d", chan, rx_control, skb->len);
4121
4122         if (__is_ctrl_final(chan, rx_control) &&
4123                         test_bit(CONN_WAIT_F, &chan->conn_state)) {
4124                 __clear_monitor_timer(chan);
4125                 if (chan->unacked_frames > 0)
4126                         __set_retrans_timer(chan);
4127                 clear_bit(CONN_WAIT_F, &chan->conn_state);
4128         }
4129
4130         switch (__get_ctrl_super(chan, rx_control)) {
4131         case L2CAP_SUPER_RR:
4132                 l2cap_data_channel_rrframe(chan, rx_control);
4133                 break;
4134
4135         case L2CAP_SUPER_REJ:
4136                 l2cap_data_channel_rejframe(chan, rx_control);
4137                 break;
4138
4139         case L2CAP_SUPER_SREJ:
4140                 l2cap_data_channel_srejframe(chan, rx_control);
4141                 break;
4142
4143         case L2CAP_SUPER_RNR:
4144                 l2cap_data_channel_rnrframe(chan, rx_control);
4145                 break;
4146         }
4147
4148         kfree_skb(skb);
4149         return 0;
4150 }
4151
4152 static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb)
4153 {
4154         struct l2cap_chan *chan = l2cap_pi(sk)->chan;
4155         u32 control;
4156         u16 req_seq;
4157         int len, next_tx_seq_offset, req_seq_offset;
4158
4159         control = __get_control(chan, skb->data);
4160         skb_pull(skb, __ctrl_size(chan));
4161         len = skb->len;
4162
4163         /*
4164          * We can just drop the corrupted I-frame here.
4165          * Receiver will miss it and start proper recovery
4166          * procedures and ask retransmission.
4167          */
4168         if (l2cap_check_fcs(chan, skb))
4169                 goto drop;
4170
4171         if (__is_sar_start(chan, control) && !__is_sframe(chan, control))
4172                 len -= L2CAP_SDULEN_SIZE;
4173
4174         if (chan->fcs == L2CAP_FCS_CRC16)
4175                 len -= L2CAP_FCS_SIZE;
4176
4177         if (len > chan->mps) {
4178                 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
4179                 goto drop;
4180         }
4181
4182         req_seq = __get_reqseq(chan, control);
4183
4184         req_seq_offset = __seq_offset(chan, req_seq, chan->expected_ack_seq);
4185
4186         next_tx_seq_offset = __seq_offset(chan, chan->next_tx_seq,
4187                                                 chan->expected_ack_seq);
4188
4189         /* check for invalid req-seq */
4190         if (req_seq_offset > next_tx_seq_offset) {
4191                 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
4192                 goto drop;
4193         }
4194
4195         if (!__is_sframe(chan, control)) {
4196                 if (len < 0) {
4197                         l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
4198                         goto drop;
4199                 }
4200
4201                 l2cap_data_channel_iframe(chan, control, skb);
4202         } else {
4203                 if (len != 0) {
4204                         BT_ERR("%d", len);
4205                         l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
4206                         goto drop;
4207                 }
4208
4209                 l2cap_data_channel_sframe(chan, control, skb);
4210         }
4211
4212         return 0;
4213
4214 drop:
4215         kfree_skb(skb);
4216         return 0;
4217 }
4218
4219 static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
4220 {
4221         struct l2cap_chan *chan;
4222         struct sock *sk = NULL;
4223         u32 control;
4224         u16 tx_seq;
4225         int len;
4226
4227         chan = l2cap_get_chan_by_scid(conn, cid);
4228         if (!chan) {
4229                 BT_DBG("unknown cid 0x%4.4x", cid);
4230                 goto drop;
4231         }
4232
4233         sk = chan->sk;
4234
4235         BT_DBG("chan %p, len %d", chan, skb->len);
4236
4237         if (chan->state != BT_CONNECTED)
4238                 goto drop;
4239
4240         switch (chan->mode) {
4241         case L2CAP_MODE_BASIC:
4242                 /* If socket recv buffers overflows we drop data here
4243                  * which is *bad* because L2CAP has to be reliable.
4244                  * But we don't have any other choice. L2CAP doesn't
4245                  * provide flow control mechanism. */
4246
4247                 if (chan->imtu < skb->len)
4248                         goto drop;
4249
4250                 if (!chan->ops->recv(chan->data, skb))
4251                         goto done;
4252                 break;
4253
4254         case L2CAP_MODE_ERTM:
4255                 l2cap_ertm_data_rcv(sk, skb);
4256
4257                 goto done;
4258
4259         case L2CAP_MODE_STREAMING:
4260                 control = __get_control(chan, skb->data);
4261                 skb_pull(skb, __ctrl_size(chan));
4262                 len = skb->len;
4263
4264                 if (l2cap_check_fcs(chan, skb))
4265                         goto drop;
4266
4267                 if (__is_sar_start(chan, control))
4268                         len -= L2CAP_SDULEN_SIZE;
4269
4270                 if (chan->fcs == L2CAP_FCS_CRC16)
4271                         len -= L2CAP_FCS_SIZE;
4272
4273                 if (len > chan->mps || len < 0 || __is_sframe(chan, control))
4274                         goto drop;
4275
4276                 tx_seq = __get_txseq(chan, control);
4277
4278                 if (chan->expected_tx_seq != tx_seq) {
4279                         /* Frame(s) missing - must discard partial SDU */
4280                         kfree_skb(chan->sdu);
4281                         chan->sdu = NULL;
4282                         chan->sdu_last_frag = NULL;
4283                         chan->sdu_len = 0;
4284
4285                         /* TODO: Notify userland of missing data */
4286                 }
4287
4288                 chan->expected_tx_seq = __next_seq(chan, tx_seq);
4289
4290                 if (l2cap_reassemble_sdu(chan, skb, control) == -EMSGSIZE)
4291                         l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
4292
4293                 goto done;
4294
4295         default:
4296                 BT_DBG("chan %p: bad mode 0x%2.2x", chan, chan->mode);
4297                 break;
4298         }
4299
4300 drop:
4301         kfree_skb(skb);
4302
4303 done:
4304         if (sk)
4305                 release_sock(sk);
4306
4307         return 0;
4308 }
4309
4310 static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb)
4311 {
4312         struct sock *sk = NULL;
4313         struct l2cap_chan *chan;
4314
4315         chan = l2cap_global_chan_by_psm(0, psm, conn->src);
4316         if (!chan)
4317                 goto drop;
4318
4319         sk = chan->sk;
4320
4321         lock_sock(sk);
4322
4323         BT_DBG("sk %p, len %d", sk, skb->len);
4324
4325         if (chan->state != BT_BOUND && chan->state != BT_CONNECTED)
4326                 goto drop;
4327
4328         if (chan->imtu < skb->len)
4329                 goto drop;
4330
4331         if (!chan->ops->recv(chan->data, skb))
4332                 goto done;
4333
4334 drop:
4335         kfree_skb(skb);
4336
4337 done:
4338         if (sk)
4339                 release_sock(sk);
4340         return 0;
4341 }
4342
4343 static inline int l2cap_att_channel(struct l2cap_conn *conn, __le16 cid, struct sk_buff *skb)
4344 {
4345         struct sock *sk = NULL;
4346         struct l2cap_chan *chan;
4347
4348         chan = l2cap_global_chan_by_scid(0, cid, conn->src);
4349         if (!chan)
4350                 goto drop;
4351
4352         sk = chan->sk;
4353
4354         lock_sock(sk);
4355
4356         BT_DBG("sk %p, len %d", sk, skb->len);
4357
4358         if (chan->state != BT_BOUND && chan->state != BT_CONNECTED)
4359                 goto drop;
4360
4361         if (chan->imtu < skb->len)
4362                 goto drop;
4363
4364         if (!chan->ops->recv(chan->data, skb))
4365                 goto done;
4366
4367 drop:
4368         kfree_skb(skb);
4369
4370 done:
4371         if (sk)
4372                 release_sock(sk);
4373         return 0;
4374 }
4375
4376 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
4377 {
4378         struct l2cap_hdr *lh = (void *) skb->data;
4379         u16 cid, len;
4380         __le16 psm;
4381
4382         skb_pull(skb, L2CAP_HDR_SIZE);
4383         cid = __le16_to_cpu(lh->cid);
4384         len = __le16_to_cpu(lh->len);
4385
4386         if (len != skb->len) {
4387                 kfree_skb(skb);
4388                 return;
4389         }
4390
4391         BT_DBG("len %d, cid 0x%4.4x", len, cid);
4392
4393         switch (cid) {
4394         case L2CAP_CID_LE_SIGNALING:
4395         case L2CAP_CID_SIGNALING:
4396                 l2cap_sig_channel(conn, skb);
4397                 break;
4398
4399         case L2CAP_CID_CONN_LESS:
4400                 psm = get_unaligned_le16(skb->data);
4401                 skb_pull(skb, 2);
4402                 l2cap_conless_channel(conn, psm, skb);
4403                 break;
4404
4405         case L2CAP_CID_LE_DATA:
4406                 l2cap_att_channel(conn, cid, skb);
4407                 break;
4408
4409         case L2CAP_CID_SMP:
4410                 if (smp_sig_channel(conn, skb))
4411                         l2cap_conn_del(conn->hcon, EACCES);
4412                 break;
4413
4414         default:
4415                 l2cap_data_channel(conn, cid, skb);
4416                 break;
4417         }
4418 }
4419
4420 /* ---- L2CAP interface with lower layer (HCI) ---- */
4421
4422 static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
4423 {
4424         int exact = 0, lm1 = 0, lm2 = 0;
4425         struct l2cap_chan *c;
4426
4427         if (type != ACL_LINK)
4428                 return -EINVAL;
4429
4430         BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
4431
4432         /* Find listening sockets and check their link_mode */
4433         read_lock(&chan_list_lock);
4434         list_for_each_entry(c, &chan_list, global_l) {
4435                 struct sock *sk = c->sk;
4436
4437                 if (c->state != BT_LISTEN)
4438                         continue;
4439
4440                 if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
4441                         lm1 |= HCI_LM_ACCEPT;
4442                         if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
4443                                 lm1 |= HCI_LM_MASTER;
4444                         exact++;
4445                 } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) {
4446                         lm2 |= HCI_LM_ACCEPT;
4447                         if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
4448                                 lm2 |= HCI_LM_MASTER;
4449                 }
4450         }
4451         read_unlock(&chan_list_lock);
4452
4453         return exact ? lm1 : lm2;
4454 }
4455
4456 static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
4457 {
4458         struct l2cap_conn *conn;
4459
4460         BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
4461
4462         if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK))
4463                 return -EINVAL;
4464
4465         if (!status) {
4466                 conn = l2cap_conn_add(hcon, status);
4467                 if (conn)
4468                         l2cap_conn_ready(conn);
4469         } else
4470                 l2cap_conn_del(hcon, bt_to_errno(status));
4471
4472         return 0;
4473 }
4474
4475 static int l2cap_disconn_ind(struct hci_conn *hcon)
4476 {
4477         struct l2cap_conn *conn = hcon->l2cap_data;
4478
4479         BT_DBG("hcon %p", hcon);
4480
4481         if ((hcon->type != ACL_LINK && hcon->type != LE_LINK) || !conn)
4482                 return HCI_ERROR_REMOTE_USER_TERM;
4483
4484         return conn->disc_reason;
4485 }
4486
4487 static int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
4488 {
4489         BT_DBG("hcon %p reason %d", hcon, reason);
4490
4491         if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK))
4492                 return -EINVAL;
4493
4494         l2cap_conn_del(hcon, bt_to_errno(reason));
4495
4496         return 0;
4497 }
4498
4499 static inline void l2cap_check_encryption(struct l2cap_chan *chan, u8 encrypt)
4500 {
4501         if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED)
4502                 return;
4503
4504         if (encrypt == 0x00) {
4505                 if (chan->sec_level == BT_SECURITY_MEDIUM) {
4506                         __clear_chan_timer(chan);
4507                         __set_chan_timer(chan, L2CAP_ENC_TIMEOUT);
4508                 } else if (chan->sec_level == BT_SECURITY_HIGH)
4509                         l2cap_chan_close(chan, ECONNREFUSED);
4510         } else {
4511                 if (chan->sec_level == BT_SECURITY_MEDIUM)
4512                         __clear_chan_timer(chan);
4513         }
4514 }
4515
4516 static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
4517 {
4518         struct l2cap_conn *conn = hcon->l2cap_data;
4519         struct l2cap_chan *chan;
4520
4521         if (!conn)
4522                 return 0;
4523
4524         BT_DBG("conn %p", conn);
4525
4526         if (hcon->type == LE_LINK) {
4527                 smp_distribute_keys(conn, 0);
4528                 del_timer(&conn->security_timer);
4529         }
4530
4531         mutex_lock(&conn->chan_lock);
4532
4533         list_for_each_entry(chan, &conn->chan_l, list) {
4534                 struct sock *sk = chan->sk;
4535
4536                 bh_lock_sock(sk);
4537
4538                 BT_DBG("chan->scid %d", chan->scid);
4539
4540                 if (chan->scid == L2CAP_CID_LE_DATA) {
4541                         if (!status && encrypt) {
4542                                 chan->sec_level = hcon->sec_level;
4543                                 l2cap_chan_ready(sk);
4544                         }
4545
4546                         bh_unlock_sock(sk);
4547                         continue;
4548                 }
4549
4550                 if (test_bit(CONF_CONNECT_PEND, &chan->conf_state)) {
4551                         bh_unlock_sock(sk);
4552                         continue;
4553                 }
4554
4555                 if (!status && (chan->state == BT_CONNECTED ||
4556                                                 chan->state == BT_CONFIG)) {
4557                         l2cap_check_encryption(chan, encrypt);
4558                         bh_unlock_sock(sk);
4559                         continue;
4560                 }
4561
4562                 if (chan->state == BT_CONNECT) {
4563                         if (!status) {
4564                                 struct l2cap_conn_req req;
4565                                 req.scid = cpu_to_le16(chan->scid);
4566                                 req.psm  = chan->psm;
4567
4568                                 chan->ident = l2cap_get_ident(conn);
4569                                 set_bit(CONF_CONNECT_PEND, &chan->conf_state);
4570
4571                                 l2cap_send_cmd(conn, chan->ident,
4572                                         L2CAP_CONN_REQ, sizeof(req), &req);
4573                         } else {
4574                                 __clear_chan_timer(chan);
4575                                 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
4576                         }
4577                 } else if (chan->state == BT_CONNECT2) {
4578                         struct l2cap_conn_rsp rsp;
4579                         __u16 res, stat;
4580
4581                         if (!status) {
4582                                 if (bt_sk(sk)->defer_setup) {
4583                                         struct sock *parent = bt_sk(sk)->parent;
4584                                         res = L2CAP_CR_PEND;
4585                                         stat = L2CAP_CS_AUTHOR_PEND;
4586                                         if (parent)
4587                                                 parent->sk_data_ready(parent, 0);
4588                                 } else {
4589                                         l2cap_state_change(chan, BT_CONFIG);
4590                                         res = L2CAP_CR_SUCCESS;
4591                                         stat = L2CAP_CS_NO_INFO;
4592                                 }
4593                         } else {
4594                                 l2cap_state_change(chan, BT_DISCONN);
4595                                 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
4596                                 res = L2CAP_CR_SEC_BLOCK;
4597                                 stat = L2CAP_CS_NO_INFO;
4598                         }
4599
4600                         rsp.scid   = cpu_to_le16(chan->dcid);
4601                         rsp.dcid   = cpu_to_le16(chan->scid);
4602                         rsp.result = cpu_to_le16(res);
4603                         rsp.status = cpu_to_le16(stat);
4604                         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
4605                                                         sizeof(rsp), &rsp);
4606                 }
4607
4608                 bh_unlock_sock(sk);
4609         }
4610
4611         mutex_unlock(&conn->chan_lock);
4612
4613         return 0;
4614 }
4615
4616 static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
4617 {
4618         struct l2cap_conn *conn = hcon->l2cap_data;
4619
4620         if (!conn)
4621                 conn = l2cap_conn_add(hcon, 0);
4622
4623         if (!conn)
4624                 goto drop;
4625
4626         BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
4627
4628         if (!(flags & ACL_CONT)) {
4629                 struct l2cap_hdr *hdr;
4630                 struct l2cap_chan *chan;
4631                 u16 cid;
4632                 int len;
4633
4634                 if (conn->rx_len) {
4635                         BT_ERR("Unexpected start frame (len %d)", skb->len);
4636                         kfree_skb(conn->rx_skb);
4637                         conn->rx_skb = NULL;
4638                         conn->rx_len = 0;
4639                         l2cap_conn_unreliable(conn, ECOMM);
4640                 }
4641
4642                 /* Start fragment always begin with Basic L2CAP header */
4643                 if (skb->len < L2CAP_HDR_SIZE) {
4644                         BT_ERR("Frame is too short (len %d)", skb->len);
4645                         l2cap_conn_unreliable(conn, ECOMM);
4646                         goto drop;
4647                 }
4648
4649                 hdr = (struct l2cap_hdr *) skb->data;
4650                 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
4651                 cid = __le16_to_cpu(hdr->cid);
4652
4653                 if (len == skb->len) {
4654                         /* Complete frame received */
4655                         l2cap_recv_frame(conn, skb);
4656                         return 0;
4657                 }
4658
4659                 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
4660
4661                 if (skb->len > len) {
4662                         BT_ERR("Frame is too long (len %d, expected len %d)",
4663                                 skb->len, len);
4664                         l2cap_conn_unreliable(conn, ECOMM);
4665                         goto drop;
4666                 }
4667
4668                 chan = l2cap_get_chan_by_scid(conn, cid);
4669
4670                 if (chan && chan->sk) {
4671                         struct sock *sk = chan->sk;
4672
4673                         if (chan->imtu < len - L2CAP_HDR_SIZE) {
4674                                 BT_ERR("Frame exceeding recv MTU (len %d, "
4675                                                         "MTU %d)", len,
4676                                                         chan->imtu);
4677                                 release_sock(sk);
4678                                 l2cap_conn_unreliable(conn, ECOMM);
4679                                 goto drop;
4680                         }
4681                         release_sock(sk);
4682                 }
4683
4684                 /* Allocate skb for the complete frame (with header) */
4685                 conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC);
4686                 if (!conn->rx_skb)
4687                         goto drop;
4688
4689                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
4690                                                                 skb->len);
4691                 conn->rx_len = len - skb->len;
4692         } else {
4693                 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
4694
4695                 if (!conn->rx_len) {
4696                         BT_ERR("Unexpected continuation frame (len %d)", skb->len);
4697                         l2cap_conn_unreliable(conn, ECOMM);
4698                         goto drop;
4699                 }
4700
4701                 if (skb->len > conn->rx_len) {
4702                         BT_ERR("Fragment is too long (len %d, expected %d)",
4703                                         skb->len, conn->rx_len);
4704                         kfree_skb(conn->rx_skb);
4705                         conn->rx_skb = NULL;
4706                         conn->rx_len = 0;
4707                         l2cap_conn_unreliable(conn, ECOMM);
4708                         goto drop;
4709                 }
4710
4711                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
4712                                                                 skb->len);
4713                 conn->rx_len -= skb->len;
4714
4715                 if (!conn->rx_len) {
4716                         /* Complete frame received */
4717                         l2cap_recv_frame(conn, conn->rx_skb);
4718                         conn->rx_skb = NULL;
4719                 }
4720         }
4721
4722 drop:
4723         kfree_skb(skb);
4724         return 0;
4725 }
4726
4727 static int l2cap_debugfs_show(struct seq_file *f, void *p)
4728 {
4729         struct l2cap_chan *c;
4730
4731         read_lock_bh(&chan_list_lock);
4732
4733         list_for_each_entry(c, &chan_list, global_l) {
4734                 struct sock *sk = c->sk;
4735
4736                 seq_printf(f, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n",
4737                                         batostr(&bt_sk(sk)->src),
4738                                         batostr(&bt_sk(sk)->dst),
4739                                         c->state, __le16_to_cpu(c->psm),
4740                                         c->scid, c->dcid, c->imtu, c->omtu,
4741                                         c->sec_level, c->mode);
4742 }
4743
4744         read_unlock_bh(&chan_list_lock);
4745
4746         return 0;
4747 }
4748
4749 static int l2cap_debugfs_open(struct inode *inode, struct file *file)
4750 {
4751         return single_open(file, l2cap_debugfs_show, inode->i_private);
4752 }
4753
4754 static const struct file_operations l2cap_debugfs_fops = {
4755         .open           = l2cap_debugfs_open,
4756         .read           = seq_read,
4757         .llseek         = seq_lseek,
4758         .release        = single_release,
4759 };
4760
4761 static struct dentry *l2cap_debugfs;
4762
4763 static struct hci_proto l2cap_hci_proto = {
4764         .name           = "L2CAP",
4765         .id             = HCI_PROTO_L2CAP,
4766         .connect_ind    = l2cap_connect_ind,
4767         .connect_cfm    = l2cap_connect_cfm,
4768         .disconn_ind    = l2cap_disconn_ind,
4769         .disconn_cfm    = l2cap_disconn_cfm,
4770         .security_cfm   = l2cap_security_cfm,
4771         .recv_acldata   = l2cap_recv_acldata
4772 };
4773
4774 int __init l2cap_init(void)
4775 {
4776         int err;
4777
4778         err = l2cap_init_sockets();
4779         if (err < 0)
4780                 return err;
4781
4782         err = hci_register_proto(&l2cap_hci_proto);
4783         if (err < 0) {
4784                 BT_ERR("L2CAP protocol registration failed");
4785                 bt_sock_unregister(BTPROTO_L2CAP);
4786                 goto error;
4787         }
4788
4789         if (bt_debugfs) {
4790                 l2cap_debugfs = debugfs_create_file("l2cap", 0444,
4791                                         bt_debugfs, NULL, &l2cap_debugfs_fops);
4792                 if (!l2cap_debugfs)
4793                         BT_ERR("Failed to create L2CAP debug file");
4794         }
4795
4796         return 0;
4797
4798 error:
4799         l2cap_cleanup_sockets();
4800         return err;
4801 }
4802
4803 void l2cap_exit(void)
4804 {
4805         debugfs_remove(l2cap_debugfs);
4806
4807         if (hci_unregister_proto(&l2cap_hci_proto) < 0)
4808                 BT_ERR("L2CAP protocol unregistration failed");
4809
4810         l2cap_cleanup_sockets();
4811 }
4812
4813 module_param(disable_ertm, bool, 0644);
4814 MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode");