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