Bluetooth: EFS: implement L2CAP config pending state
[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 and mark state
2244                                    local PENDING */
2245                                 result = L2CAP_CONF_PENDING;
2246                                 set_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
2247                         }
2248                 }
2249
2250                 switch (rfc.mode) {
2251                 case L2CAP_MODE_BASIC:
2252                         chan->fcs = L2CAP_FCS_NONE;
2253                         set_bit(CONF_MODE_DONE, &chan->conf_state);
2254                         break;
2255
2256                 case L2CAP_MODE_ERTM:
2257                         if (!test_bit(CONF_EWS_RECV, &chan->conf_state))
2258                                 chan->remote_tx_win = rfc.txwin_size;
2259                         else
2260                                 rfc.txwin_size = L2CAP_DEFAULT_TX_WINDOW;
2261
2262                         chan->remote_max_tx = rfc.max_transmit;
2263
2264                         size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
2265                                                 chan->conn->mtu -
2266                                                 L2CAP_EXT_HDR_SIZE -
2267                                                 L2CAP_SDULEN_SIZE -
2268                                                 L2CAP_FCS_SIZE);
2269                         rfc.max_pdu_size = cpu_to_le16(size);
2270                         chan->remote_mps = size;
2271
2272                         rfc.retrans_timeout =
2273                                 le16_to_cpu(L2CAP_DEFAULT_RETRANS_TO);
2274                         rfc.monitor_timeout =
2275                                 le16_to_cpu(L2CAP_DEFAULT_MONITOR_TO);
2276
2277                         set_bit(CONF_MODE_DONE, &chan->conf_state);
2278
2279                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2280                                         sizeof(rfc), (unsigned long) &rfc);
2281
2282                         if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
2283                                 chan->remote_id = efs.id;
2284                                 chan->remote_stype = efs.stype;
2285                                 chan->remote_msdu = le16_to_cpu(efs.msdu);
2286                                 chan->remote_flush_to =
2287                                                 le32_to_cpu(efs.flush_to);
2288                                 chan->remote_acc_lat =
2289                                                 le32_to_cpu(efs.acc_lat);
2290                                 chan->remote_sdu_itime =
2291                                         le32_to_cpu(efs.sdu_itime);
2292                                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
2293                                         sizeof(efs), (unsigned long) &efs);
2294                         }
2295                         break;
2296
2297                 case L2CAP_MODE_STREAMING:
2298                         size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
2299                                                 chan->conn->mtu -
2300                                                 L2CAP_EXT_HDR_SIZE -
2301                                                 L2CAP_SDULEN_SIZE -
2302                                                 L2CAP_FCS_SIZE);
2303                         rfc.max_pdu_size = cpu_to_le16(size);
2304                         chan->remote_mps = size;
2305
2306                         set_bit(CONF_MODE_DONE, &chan->conf_state);
2307
2308                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2309                                         sizeof(rfc), (unsigned long) &rfc);
2310
2311                         break;
2312
2313                 default:
2314                         result = L2CAP_CONF_UNACCEPT;
2315
2316                         memset(&rfc, 0, sizeof(rfc));
2317                         rfc.mode = chan->mode;
2318                 }
2319
2320                 if (result == L2CAP_CONF_SUCCESS)
2321                         set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
2322         }
2323         rsp->scid   = cpu_to_le16(chan->dcid);
2324         rsp->result = cpu_to_le16(result);
2325         rsp->flags  = cpu_to_le16(0x0000);
2326
2327         return ptr - data;
2328 }
2329
2330 static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, void *data, u16 *result)
2331 {
2332         struct l2cap_conf_req *req = data;
2333         void *ptr = req->data;
2334         int type, olen;
2335         unsigned long val;
2336         struct l2cap_conf_rfc rfc;
2337
2338         BT_DBG("chan %p, rsp %p, len %d, req %p", chan, rsp, len, data);
2339
2340         while (len >= L2CAP_CONF_OPT_SIZE) {
2341                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
2342
2343                 switch (type) {
2344                 case L2CAP_CONF_MTU:
2345                         if (val < L2CAP_DEFAULT_MIN_MTU) {
2346                                 *result = L2CAP_CONF_UNACCEPT;
2347                                 chan->imtu = L2CAP_DEFAULT_MIN_MTU;
2348                         } else
2349                                 chan->imtu = val;
2350                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu);
2351                         break;
2352
2353                 case L2CAP_CONF_FLUSH_TO:
2354                         chan->flush_to = val;
2355                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
2356                                                         2, chan->flush_to);
2357                         break;
2358
2359                 case L2CAP_CONF_RFC:
2360                         if (olen == sizeof(rfc))
2361                                 memcpy(&rfc, (void *)val, olen);
2362
2363                         if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state) &&
2364                                                         rfc.mode != chan->mode)
2365                                 return -ECONNREFUSED;
2366
2367                         chan->fcs = 0;
2368
2369                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2370                                         sizeof(rfc), (unsigned long) &rfc);
2371                         break;
2372
2373                 case L2CAP_CONF_EWS:
2374                         chan->tx_win = min_t(u16, val,
2375                                                 L2CAP_DEFAULT_EXT_WINDOW);
2376                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS,
2377                                                         2, chan->tx_win);
2378                         break;
2379                 }
2380         }
2381
2382         if (chan->mode == L2CAP_MODE_BASIC && chan->mode != rfc.mode)
2383                 return -ECONNREFUSED;
2384
2385         chan->mode = rfc.mode;
2386
2387         if (*result == L2CAP_CONF_SUCCESS || *result == L2CAP_CONF_PENDING) {
2388                 switch (rfc.mode) {
2389                 case L2CAP_MODE_ERTM:
2390                         chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
2391                         chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
2392                         chan->mps    = le16_to_cpu(rfc.max_pdu_size);
2393                         break;
2394                 case L2CAP_MODE_STREAMING:
2395                         chan->mps    = le16_to_cpu(rfc.max_pdu_size);
2396                 }
2397         }
2398
2399         req->dcid   = cpu_to_le16(chan->dcid);
2400         req->flags  = cpu_to_le16(0x0000);
2401
2402         return ptr - data;
2403 }
2404
2405 static int l2cap_build_conf_rsp(struct l2cap_chan *chan, void *data, u16 result, u16 flags)
2406 {
2407         struct l2cap_conf_rsp *rsp = data;
2408         void *ptr = rsp->data;
2409
2410         BT_DBG("chan %p", chan);
2411
2412         rsp->scid   = cpu_to_le16(chan->dcid);
2413         rsp->result = cpu_to_le16(result);
2414         rsp->flags  = cpu_to_le16(flags);
2415
2416         return ptr - data;
2417 }
2418
2419 void __l2cap_connect_rsp_defer(struct l2cap_chan *chan)
2420 {
2421         struct l2cap_conn_rsp rsp;
2422         struct l2cap_conn *conn = chan->conn;
2423         u8 buf[128];
2424
2425         rsp.scid   = cpu_to_le16(chan->dcid);
2426         rsp.dcid   = cpu_to_le16(chan->scid);
2427         rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
2428         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
2429         l2cap_send_cmd(conn, chan->ident,
2430                                 L2CAP_CONN_RSP, sizeof(rsp), &rsp);
2431
2432         if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
2433                 return;
2434
2435         l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2436                         l2cap_build_conf_req(chan, buf), buf);
2437         chan->num_conf_req++;
2438 }
2439
2440 static void l2cap_conf_rfc_get(struct l2cap_chan *chan, void *rsp, int len)
2441 {
2442         int type, olen;
2443         unsigned long val;
2444         struct l2cap_conf_rfc rfc;
2445
2446         BT_DBG("chan %p, rsp %p, len %d", chan, rsp, len);
2447
2448         if ((chan->mode != L2CAP_MODE_ERTM) && (chan->mode != L2CAP_MODE_STREAMING))
2449                 return;
2450
2451         while (len >= L2CAP_CONF_OPT_SIZE) {
2452                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
2453
2454                 switch (type) {
2455                 case L2CAP_CONF_RFC:
2456                         if (olen == sizeof(rfc))
2457                                 memcpy(&rfc, (void *)val, olen);
2458                         goto done;
2459                 }
2460         }
2461
2462 done:
2463         switch (rfc.mode) {
2464         case L2CAP_MODE_ERTM:
2465                 chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
2466                 chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
2467                 chan->mps    = le16_to_cpu(rfc.max_pdu_size);
2468                 break;
2469         case L2CAP_MODE_STREAMING:
2470                 chan->mps    = le16_to_cpu(rfc.max_pdu_size);
2471         }
2472 }
2473
2474 static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2475 {
2476         struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data;
2477
2478         if (rej->reason != L2CAP_REJ_NOT_UNDERSTOOD)
2479                 return 0;
2480
2481         if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
2482                                         cmd->ident == conn->info_ident) {
2483                 del_timer(&conn->info_timer);
2484
2485                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2486                 conn->info_ident = 0;
2487
2488                 l2cap_conn_start(conn);
2489         }
2490
2491         return 0;
2492 }
2493
2494 static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2495 {
2496         struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
2497         struct l2cap_conn_rsp rsp;
2498         struct l2cap_chan *chan = NULL, *pchan;
2499         struct sock *parent, *sk = NULL;
2500         int result, status = L2CAP_CS_NO_INFO;
2501
2502         u16 dcid = 0, scid = __le16_to_cpu(req->scid);
2503         __le16 psm = req->psm;
2504
2505         BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid);
2506
2507         /* Check if we have socket listening on psm */
2508         pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, conn->src);
2509         if (!pchan) {
2510                 result = L2CAP_CR_BAD_PSM;
2511                 goto sendresp;
2512         }
2513
2514         parent = pchan->sk;
2515
2516         bh_lock_sock(parent);
2517
2518         /* Check if the ACL is secure enough (if not SDP) */
2519         if (psm != cpu_to_le16(0x0001) &&
2520                                 !hci_conn_check_link_mode(conn->hcon)) {
2521                 conn->disc_reason = 0x05;
2522                 result = L2CAP_CR_SEC_BLOCK;
2523                 goto response;
2524         }
2525
2526         result = L2CAP_CR_NO_MEM;
2527
2528         /* Check for backlog size */
2529         if (sk_acceptq_is_full(parent)) {
2530                 BT_DBG("backlog full %d", parent->sk_ack_backlog);
2531                 goto response;
2532         }
2533
2534         chan = pchan->ops->new_connection(pchan->data);
2535         if (!chan)
2536                 goto response;
2537
2538         sk = chan->sk;
2539
2540         write_lock_bh(&conn->chan_lock);
2541
2542         /* Check if we already have channel with that dcid */
2543         if (__l2cap_get_chan_by_dcid(conn, scid)) {
2544                 write_unlock_bh(&conn->chan_lock);
2545                 sock_set_flag(sk, SOCK_ZAPPED);
2546                 chan->ops->close(chan->data);
2547                 goto response;
2548         }
2549
2550         hci_conn_hold(conn->hcon);
2551
2552         bacpy(&bt_sk(sk)->src, conn->src);
2553         bacpy(&bt_sk(sk)->dst, conn->dst);
2554         chan->psm  = psm;
2555         chan->dcid = scid;
2556
2557         bt_accept_enqueue(parent, sk);
2558
2559         __l2cap_chan_add(conn, chan);
2560
2561         dcid = chan->scid;
2562
2563         __set_chan_timer(chan, sk->sk_sndtimeo);
2564
2565         chan->ident = cmd->ident;
2566
2567         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
2568                 if (l2cap_check_security(chan)) {
2569                         if (bt_sk(sk)->defer_setup) {
2570                                 l2cap_state_change(chan, BT_CONNECT2);
2571                                 result = L2CAP_CR_PEND;
2572                                 status = L2CAP_CS_AUTHOR_PEND;
2573                                 parent->sk_data_ready(parent, 0);
2574                         } else {
2575                                 l2cap_state_change(chan, BT_CONFIG);
2576                                 result = L2CAP_CR_SUCCESS;
2577                                 status = L2CAP_CS_NO_INFO;
2578                         }
2579                 } else {
2580                         l2cap_state_change(chan, BT_CONNECT2);
2581                         result = L2CAP_CR_PEND;
2582                         status = L2CAP_CS_AUTHEN_PEND;
2583                 }
2584         } else {
2585                 l2cap_state_change(chan, BT_CONNECT2);
2586                 result = L2CAP_CR_PEND;
2587                 status = L2CAP_CS_NO_INFO;
2588         }
2589
2590         write_unlock_bh(&conn->chan_lock);
2591
2592 response:
2593         bh_unlock_sock(parent);
2594
2595 sendresp:
2596         rsp.scid   = cpu_to_le16(scid);
2597         rsp.dcid   = cpu_to_le16(dcid);
2598         rsp.result = cpu_to_le16(result);
2599         rsp.status = cpu_to_le16(status);
2600         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
2601
2602         if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
2603                 struct l2cap_info_req info;
2604                 info.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
2605
2606                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
2607                 conn->info_ident = l2cap_get_ident(conn);
2608
2609                 mod_timer(&conn->info_timer, jiffies +
2610                                         msecs_to_jiffies(L2CAP_INFO_TIMEOUT));
2611
2612                 l2cap_send_cmd(conn, conn->info_ident,
2613                                         L2CAP_INFO_REQ, sizeof(info), &info);
2614         }
2615
2616         if (chan && !test_bit(CONF_REQ_SENT, &chan->conf_state) &&
2617                                 result == L2CAP_CR_SUCCESS) {
2618                 u8 buf[128];
2619                 set_bit(CONF_REQ_SENT, &chan->conf_state);
2620                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2621                                         l2cap_build_conf_req(chan, buf), buf);
2622                 chan->num_conf_req++;
2623         }
2624
2625         return 0;
2626 }
2627
2628 static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2629 {
2630         struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
2631         u16 scid, dcid, result, status;
2632         struct l2cap_chan *chan;
2633         struct sock *sk;
2634         u8 req[128];
2635
2636         scid   = __le16_to_cpu(rsp->scid);
2637         dcid   = __le16_to_cpu(rsp->dcid);
2638         result = __le16_to_cpu(rsp->result);
2639         status = __le16_to_cpu(rsp->status);
2640
2641         BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", dcid, scid, result, status);
2642
2643         if (scid) {
2644                 chan = l2cap_get_chan_by_scid(conn, scid);
2645                 if (!chan)
2646                         return -EFAULT;
2647         } else {
2648                 chan = l2cap_get_chan_by_ident(conn, cmd->ident);
2649                 if (!chan)
2650                         return -EFAULT;
2651         }
2652
2653         sk = chan->sk;
2654
2655         switch (result) {
2656         case L2CAP_CR_SUCCESS:
2657                 l2cap_state_change(chan, BT_CONFIG);
2658                 chan->ident = 0;
2659                 chan->dcid = dcid;
2660                 clear_bit(CONF_CONNECT_PEND, &chan->conf_state);
2661
2662                 if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
2663                         break;
2664
2665                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2666                                         l2cap_build_conf_req(chan, req), req);
2667                 chan->num_conf_req++;
2668                 break;
2669
2670         case L2CAP_CR_PEND:
2671                 set_bit(CONF_CONNECT_PEND, &chan->conf_state);
2672                 break;
2673
2674         default:
2675                 /* don't delete l2cap channel if sk is owned by user */
2676                 if (sock_owned_by_user(sk)) {
2677                         l2cap_state_change(chan, BT_DISCONN);
2678                         __clear_chan_timer(chan);
2679                         __set_chan_timer(chan, HZ / 5);
2680                         break;
2681                 }
2682
2683                 l2cap_chan_del(chan, ECONNREFUSED);
2684                 break;
2685         }
2686
2687         bh_unlock_sock(sk);
2688         return 0;
2689 }
2690
2691 static inline void set_default_fcs(struct l2cap_chan *chan)
2692 {
2693         /* FCS is enabled only in ERTM or streaming mode, if one or both
2694          * sides request it.
2695          */
2696         if (chan->mode != L2CAP_MODE_ERTM && chan->mode != L2CAP_MODE_STREAMING)
2697                 chan->fcs = L2CAP_FCS_NONE;
2698         else if (!test_bit(CONF_NO_FCS_RECV, &chan->conf_state))
2699                 chan->fcs = L2CAP_FCS_CRC16;
2700 }
2701
2702 static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
2703 {
2704         struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
2705         u16 dcid, flags;
2706         u8 rsp[64];
2707         struct l2cap_chan *chan;
2708         struct sock *sk;
2709         int len;
2710
2711         dcid  = __le16_to_cpu(req->dcid);
2712         flags = __le16_to_cpu(req->flags);
2713
2714         BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
2715
2716         chan = l2cap_get_chan_by_scid(conn, dcid);
2717         if (!chan)
2718                 return -ENOENT;
2719
2720         sk = chan->sk;
2721
2722         if (chan->state != BT_CONFIG && chan->state != BT_CONNECT2) {
2723                 struct l2cap_cmd_rej_cid rej;
2724
2725                 rej.reason = cpu_to_le16(L2CAP_REJ_INVALID_CID);
2726                 rej.scid = cpu_to_le16(chan->scid);
2727                 rej.dcid = cpu_to_le16(chan->dcid);
2728
2729                 l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ,
2730                                 sizeof(rej), &rej);
2731                 goto unlock;
2732         }
2733
2734         /* Reject if config buffer is too small. */
2735         len = cmd_len - sizeof(*req);
2736         if (len < 0 || chan->conf_len + len > sizeof(chan->conf_req)) {
2737                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2738                                 l2cap_build_conf_rsp(chan, rsp,
2739                                         L2CAP_CONF_REJECT, flags), rsp);
2740                 goto unlock;
2741         }
2742
2743         /* Store config. */
2744         memcpy(chan->conf_req + chan->conf_len, req->data, len);
2745         chan->conf_len += len;
2746
2747         if (flags & 0x0001) {
2748                 /* Incomplete config. Send empty response. */
2749                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2750                                 l2cap_build_conf_rsp(chan, rsp,
2751                                         L2CAP_CONF_SUCCESS, 0x0001), rsp);
2752                 goto unlock;
2753         }
2754
2755         /* Complete config. */
2756         len = l2cap_parse_conf_req(chan, rsp);
2757         if (len < 0) {
2758                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
2759                 goto unlock;
2760         }
2761
2762         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
2763         chan->num_conf_rsp++;
2764
2765         /* Reset config buffer. */
2766         chan->conf_len = 0;
2767
2768         if (!test_bit(CONF_OUTPUT_DONE, &chan->conf_state))
2769                 goto unlock;
2770
2771         if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
2772                 set_default_fcs(chan);
2773
2774                 l2cap_state_change(chan, BT_CONNECTED);
2775
2776                 chan->next_tx_seq = 0;
2777                 chan->expected_tx_seq = 0;
2778                 skb_queue_head_init(&chan->tx_q);
2779                 if (chan->mode == L2CAP_MODE_ERTM)
2780                         l2cap_ertm_init(chan);
2781
2782                 l2cap_chan_ready(sk);
2783                 goto unlock;
2784         }
2785
2786         if (!test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) {
2787                 u8 buf[64];
2788                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2789                                         l2cap_build_conf_req(chan, buf), buf);
2790                 chan->num_conf_req++;
2791         }
2792
2793         /* Got Conf Rsp PENDING from remote side and asume we sent
2794            Conf Rsp PENDING in the code above */
2795         if (test_bit(CONF_REM_CONF_PEND, &chan->conf_state) &&
2796                         test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
2797
2798                 /* check compatibility */
2799
2800                 clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
2801                 set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
2802
2803                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2804                                 l2cap_build_conf_rsp(chan, rsp,
2805                                         L2CAP_CONF_SUCCESS, 0x0000), rsp);
2806         }
2807
2808 unlock:
2809         bh_unlock_sock(sk);
2810         return 0;
2811 }
2812
2813 static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2814 {
2815         struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
2816         u16 scid, flags, result;
2817         struct l2cap_chan *chan;
2818         struct sock *sk;
2819         int len = cmd->len - sizeof(*rsp);
2820
2821         scid   = __le16_to_cpu(rsp->scid);
2822         flags  = __le16_to_cpu(rsp->flags);
2823         result = __le16_to_cpu(rsp->result);
2824
2825         BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x",
2826                         scid, flags, result);
2827
2828         chan = l2cap_get_chan_by_scid(conn, scid);
2829         if (!chan)
2830                 return 0;
2831
2832         sk = chan->sk;
2833
2834         switch (result) {
2835         case L2CAP_CONF_SUCCESS:
2836                 l2cap_conf_rfc_get(chan, rsp->data, len);
2837                 clear_bit(CONF_REM_CONF_PEND, &chan->conf_state);
2838                 break;
2839
2840         case L2CAP_CONF_PENDING:
2841                 set_bit(CONF_REM_CONF_PEND, &chan->conf_state);
2842
2843                 if (test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
2844                         char buf[64];
2845
2846                         len = l2cap_parse_conf_rsp(chan, rsp->data, len,
2847                                                                 buf, &result);
2848                         if (len < 0) {
2849                                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
2850                                 goto done;
2851                         }
2852
2853                         /* check compatibility */
2854
2855                         clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
2856                         set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
2857
2858                         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2859                                         l2cap_build_conf_rsp(chan, buf,
2860                                                 L2CAP_CONF_SUCCESS, 0x0000), buf);
2861                 }
2862                 goto done;
2863
2864         case L2CAP_CONF_UNACCEPT:
2865                 if (chan->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
2866                         char req[64];
2867
2868                         if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) {
2869                                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
2870                                 goto done;
2871                         }
2872
2873                         /* throw out any old stored conf requests */
2874                         result = L2CAP_CONF_SUCCESS;
2875                         len = l2cap_parse_conf_rsp(chan, rsp->data, len,
2876                                                                 req, &result);
2877                         if (len < 0) {
2878                                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
2879                                 goto done;
2880                         }
2881
2882                         l2cap_send_cmd(conn, l2cap_get_ident(conn),
2883                                                 L2CAP_CONF_REQ, len, req);
2884                         chan->num_conf_req++;
2885                         if (result != L2CAP_CONF_SUCCESS)
2886                                 goto done;
2887                         break;
2888                 }
2889
2890         default:
2891                 sk->sk_err = ECONNRESET;
2892                 __set_chan_timer(chan, HZ * 5);
2893                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
2894                 goto done;
2895         }
2896
2897         if (flags & 0x01)
2898                 goto done;
2899
2900         set_bit(CONF_INPUT_DONE, &chan->conf_state);
2901
2902         if (test_bit(CONF_OUTPUT_DONE, &chan->conf_state)) {
2903                 set_default_fcs(chan);
2904
2905                 l2cap_state_change(chan, BT_CONNECTED);
2906                 chan->next_tx_seq = 0;
2907                 chan->expected_tx_seq = 0;
2908                 skb_queue_head_init(&chan->tx_q);
2909                 if (chan->mode ==  L2CAP_MODE_ERTM)
2910                         l2cap_ertm_init(chan);
2911
2912                 l2cap_chan_ready(sk);
2913         }
2914
2915 done:
2916         bh_unlock_sock(sk);
2917         return 0;
2918 }
2919
2920 static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2921 {
2922         struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
2923         struct l2cap_disconn_rsp rsp;
2924         u16 dcid, scid;
2925         struct l2cap_chan *chan;
2926         struct sock *sk;
2927
2928         scid = __le16_to_cpu(req->scid);
2929         dcid = __le16_to_cpu(req->dcid);
2930
2931         BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
2932
2933         chan = l2cap_get_chan_by_scid(conn, dcid);
2934         if (!chan)
2935                 return 0;
2936
2937         sk = chan->sk;
2938
2939         rsp.dcid = cpu_to_le16(chan->scid);
2940         rsp.scid = cpu_to_le16(chan->dcid);
2941         l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
2942
2943         sk->sk_shutdown = SHUTDOWN_MASK;
2944
2945         /* don't delete l2cap channel if sk is owned by user */
2946         if (sock_owned_by_user(sk)) {
2947                 l2cap_state_change(chan, BT_DISCONN);
2948                 __clear_chan_timer(chan);
2949                 __set_chan_timer(chan, HZ / 5);
2950                 bh_unlock_sock(sk);
2951                 return 0;
2952         }
2953
2954         l2cap_chan_del(chan, ECONNRESET);
2955         bh_unlock_sock(sk);
2956
2957         chan->ops->close(chan->data);
2958         return 0;
2959 }
2960
2961 static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2962 {
2963         struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
2964         u16 dcid, scid;
2965         struct l2cap_chan *chan;
2966         struct sock *sk;
2967
2968         scid = __le16_to_cpu(rsp->scid);
2969         dcid = __le16_to_cpu(rsp->dcid);
2970
2971         BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
2972
2973         chan = l2cap_get_chan_by_scid(conn, scid);
2974         if (!chan)
2975                 return 0;
2976
2977         sk = chan->sk;
2978
2979         /* don't delete l2cap channel if sk is owned by user */
2980         if (sock_owned_by_user(sk)) {
2981                 l2cap_state_change(chan,BT_DISCONN);
2982                 __clear_chan_timer(chan);
2983                 __set_chan_timer(chan, HZ / 5);
2984                 bh_unlock_sock(sk);
2985                 return 0;
2986         }
2987
2988         l2cap_chan_del(chan, 0);
2989         bh_unlock_sock(sk);
2990
2991         chan->ops->close(chan->data);
2992         return 0;
2993 }
2994
2995 static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2996 {
2997         struct l2cap_info_req *req = (struct l2cap_info_req *) data;
2998         u16 type;
2999
3000         type = __le16_to_cpu(req->type);
3001
3002         BT_DBG("type 0x%4.4x", type);
3003
3004         if (type == L2CAP_IT_FEAT_MASK) {
3005                 u8 buf[8];
3006                 u32 feat_mask = l2cap_feat_mask;
3007                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
3008                 rsp->type   = cpu_to_le16(L2CAP_IT_FEAT_MASK);
3009                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
3010                 if (!disable_ertm)
3011                         feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
3012                                                          | L2CAP_FEAT_FCS;
3013                 if (enable_hs)
3014                         feat_mask |= L2CAP_FEAT_EXT_FLOW
3015                                                 | L2CAP_FEAT_EXT_WINDOW;
3016
3017                 put_unaligned_le32(feat_mask, rsp->data);
3018                 l2cap_send_cmd(conn, cmd->ident,
3019                                         L2CAP_INFO_RSP, sizeof(buf), buf);
3020         } else if (type == L2CAP_IT_FIXED_CHAN) {
3021                 u8 buf[12];
3022                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
3023                 rsp->type   = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
3024                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
3025                 memcpy(buf + 4, l2cap_fixed_chan, 8);
3026                 l2cap_send_cmd(conn, cmd->ident,
3027                                         L2CAP_INFO_RSP, sizeof(buf), buf);
3028         } else {
3029                 struct l2cap_info_rsp rsp;
3030                 rsp.type   = cpu_to_le16(type);
3031                 rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP);
3032                 l2cap_send_cmd(conn, cmd->ident,
3033                                         L2CAP_INFO_RSP, sizeof(rsp), &rsp);
3034         }
3035
3036         return 0;
3037 }
3038
3039 static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3040 {
3041         struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
3042         u16 type, result;
3043
3044         type   = __le16_to_cpu(rsp->type);
3045         result = __le16_to_cpu(rsp->result);
3046
3047         BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
3048
3049         /* L2CAP Info req/rsp are unbound to channels, add extra checks */
3050         if (cmd->ident != conn->info_ident ||
3051                         conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)
3052                 return 0;
3053
3054         del_timer(&conn->info_timer);
3055
3056         if (result != L2CAP_IR_SUCCESS) {
3057                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3058                 conn->info_ident = 0;
3059
3060                 l2cap_conn_start(conn);
3061
3062                 return 0;
3063         }
3064
3065         if (type == L2CAP_IT_FEAT_MASK) {
3066                 conn->feat_mask = get_unaligned_le32(rsp->data);
3067
3068                 if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
3069                         struct l2cap_info_req req;
3070                         req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
3071
3072                         conn->info_ident = l2cap_get_ident(conn);
3073
3074                         l2cap_send_cmd(conn, conn->info_ident,
3075                                         L2CAP_INFO_REQ, sizeof(req), &req);
3076                 } else {
3077                         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3078                         conn->info_ident = 0;
3079
3080                         l2cap_conn_start(conn);
3081                 }
3082         } else if (type == L2CAP_IT_FIXED_CHAN) {
3083                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3084                 conn->info_ident = 0;
3085
3086                 l2cap_conn_start(conn);
3087         }
3088
3089         return 0;
3090 }
3091
3092 static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency,
3093                                                         u16 to_multiplier)
3094 {
3095         u16 max_latency;
3096
3097         if (min > max || min < 6 || max > 3200)
3098                 return -EINVAL;
3099
3100         if (to_multiplier < 10 || to_multiplier > 3200)
3101                 return -EINVAL;
3102
3103         if (max >= to_multiplier * 8)
3104                 return -EINVAL;
3105
3106         max_latency = (to_multiplier * 8 / max) - 1;
3107         if (latency > 499 || latency > max_latency)
3108                 return -EINVAL;
3109
3110         return 0;
3111 }
3112
3113 static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
3114                                         struct l2cap_cmd_hdr *cmd, u8 *data)
3115 {
3116         struct hci_conn *hcon = conn->hcon;
3117         struct l2cap_conn_param_update_req *req;
3118         struct l2cap_conn_param_update_rsp rsp;
3119         u16 min, max, latency, to_multiplier, cmd_len;
3120         int err;
3121
3122         if (!(hcon->link_mode & HCI_LM_MASTER))
3123                 return -EINVAL;
3124
3125         cmd_len = __le16_to_cpu(cmd->len);
3126         if (cmd_len != sizeof(struct l2cap_conn_param_update_req))
3127                 return -EPROTO;
3128
3129         req = (struct l2cap_conn_param_update_req *) data;
3130         min             = __le16_to_cpu(req->min);
3131         max             = __le16_to_cpu(req->max);
3132         latency         = __le16_to_cpu(req->latency);
3133         to_multiplier   = __le16_to_cpu(req->to_multiplier);
3134
3135         BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x",
3136                                                 min, max, latency, to_multiplier);
3137
3138         memset(&rsp, 0, sizeof(rsp));
3139
3140         err = l2cap_check_conn_param(min, max, latency, to_multiplier);
3141         if (err)
3142                 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_REJECTED);
3143         else
3144                 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED);
3145
3146         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP,
3147                                                         sizeof(rsp), &rsp);
3148
3149         if (!err)
3150                 hci_le_conn_update(hcon, min, max, latency, to_multiplier);
3151
3152         return 0;
3153 }
3154
3155 static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
3156                         struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
3157 {
3158         int err = 0;
3159
3160         switch (cmd->code) {
3161         case L2CAP_COMMAND_REJ:
3162                 l2cap_command_rej(conn, cmd, data);
3163                 break;
3164
3165         case L2CAP_CONN_REQ:
3166                 err = l2cap_connect_req(conn, cmd, data);
3167                 break;
3168
3169         case L2CAP_CONN_RSP:
3170                 err = l2cap_connect_rsp(conn, cmd, data);
3171                 break;
3172
3173         case L2CAP_CONF_REQ:
3174                 err = l2cap_config_req(conn, cmd, cmd_len, data);
3175                 break;
3176
3177         case L2CAP_CONF_RSP:
3178                 err = l2cap_config_rsp(conn, cmd, data);
3179                 break;
3180
3181         case L2CAP_DISCONN_REQ:
3182                 err = l2cap_disconnect_req(conn, cmd, data);
3183                 break;
3184
3185         case L2CAP_DISCONN_RSP:
3186                 err = l2cap_disconnect_rsp(conn, cmd, data);
3187                 break;
3188
3189         case L2CAP_ECHO_REQ:
3190                 l2cap_send_cmd(conn, cmd->ident, L2CAP_ECHO_RSP, cmd_len, data);
3191                 break;
3192
3193         case L2CAP_ECHO_RSP:
3194                 break;
3195
3196         case L2CAP_INFO_REQ:
3197                 err = l2cap_information_req(conn, cmd, data);
3198                 break;
3199
3200         case L2CAP_INFO_RSP:
3201                 err = l2cap_information_rsp(conn, cmd, data);
3202                 break;
3203
3204         default:
3205                 BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code);
3206                 err = -EINVAL;
3207                 break;
3208         }
3209
3210         return err;
3211 }
3212
3213 static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn,
3214                                         struct l2cap_cmd_hdr *cmd, u8 *data)
3215 {
3216         switch (cmd->code) {
3217         case L2CAP_COMMAND_REJ:
3218                 return 0;
3219
3220         case L2CAP_CONN_PARAM_UPDATE_REQ:
3221                 return l2cap_conn_param_update_req(conn, cmd, data);
3222
3223         case L2CAP_CONN_PARAM_UPDATE_RSP:
3224                 return 0;
3225
3226         default:
3227                 BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code);
3228                 return -EINVAL;
3229         }
3230 }
3231
3232 static inline void l2cap_sig_channel(struct l2cap_conn *conn,
3233                                                         struct sk_buff *skb)
3234 {
3235         u8 *data = skb->data;
3236         int len = skb->len;
3237         struct l2cap_cmd_hdr cmd;
3238         int err;
3239
3240         l2cap_raw_recv(conn, skb);
3241
3242         while (len >= L2CAP_CMD_HDR_SIZE) {
3243                 u16 cmd_len;
3244                 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
3245                 data += L2CAP_CMD_HDR_SIZE;
3246                 len  -= L2CAP_CMD_HDR_SIZE;
3247
3248                 cmd_len = le16_to_cpu(cmd.len);
3249
3250                 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len, cmd.ident);
3251
3252                 if (cmd_len > len || !cmd.ident) {
3253                         BT_DBG("corrupted command");
3254                         break;
3255                 }
3256
3257                 if (conn->hcon->type == LE_LINK)
3258                         err = l2cap_le_sig_cmd(conn, &cmd, data);
3259                 else
3260                         err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data);
3261
3262                 if (err) {
3263                         struct l2cap_cmd_rej_unk rej;
3264
3265                         BT_ERR("Wrong link type (%d)", err);
3266
3267                         /* FIXME: Map err to a valid reason */
3268                         rej.reason = cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
3269                         l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
3270                 }
3271
3272                 data += cmd_len;
3273                 len  -= cmd_len;
3274         }
3275
3276         kfree_skb(skb);
3277 }
3278
3279 static int l2cap_check_fcs(struct l2cap_chan *chan,  struct sk_buff *skb)
3280 {
3281         u16 our_fcs, rcv_fcs;
3282         int hdr_size;
3283
3284         if (test_bit(FLAG_EXT_CTRL, &chan->flags))
3285                 hdr_size = L2CAP_EXT_HDR_SIZE;
3286         else
3287                 hdr_size = L2CAP_ENH_HDR_SIZE;
3288
3289         if (chan->fcs == L2CAP_FCS_CRC16) {
3290                 skb_trim(skb, skb->len - L2CAP_FCS_SIZE);
3291                 rcv_fcs = get_unaligned_le16(skb->data + skb->len);
3292                 our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
3293
3294                 if (our_fcs != rcv_fcs)
3295                         return -EBADMSG;
3296         }
3297         return 0;
3298 }
3299
3300 static inline void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan)
3301 {
3302         u32 control = 0;
3303
3304         chan->frames_sent = 0;
3305
3306         control |= __set_reqseq(chan, chan->buffer_seq);
3307
3308         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
3309                 control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR);
3310                 l2cap_send_sframe(chan, control);
3311                 set_bit(CONN_RNR_SENT, &chan->conn_state);
3312         }
3313
3314         if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
3315                 l2cap_retransmit_frames(chan);
3316
3317         l2cap_ertm_send(chan);
3318
3319         if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state) &&
3320                         chan->frames_sent == 0) {
3321                 control |= __set_ctrl_super(chan, L2CAP_SUPER_RR);
3322                 l2cap_send_sframe(chan, control);
3323         }
3324 }
3325
3326 static int l2cap_add_to_srej_queue(struct l2cap_chan *chan, struct sk_buff *skb, u16 tx_seq, u8 sar)
3327 {
3328         struct sk_buff *next_skb;
3329         int tx_seq_offset, next_tx_seq_offset;
3330
3331         bt_cb(skb)->tx_seq = tx_seq;
3332         bt_cb(skb)->sar = sar;
3333
3334         next_skb = skb_peek(&chan->srej_q);
3335         if (!next_skb) {
3336                 __skb_queue_tail(&chan->srej_q, skb);
3337                 return 0;
3338         }
3339
3340         tx_seq_offset = __seq_offset(chan, tx_seq, chan->buffer_seq);
3341
3342         do {
3343                 if (bt_cb(next_skb)->tx_seq == tx_seq)
3344                         return -EINVAL;
3345
3346                 next_tx_seq_offset = __seq_offset(chan,
3347                                 bt_cb(next_skb)->tx_seq, chan->buffer_seq);
3348
3349                 if (next_tx_seq_offset > tx_seq_offset) {
3350                         __skb_queue_before(&chan->srej_q, next_skb, skb);
3351                         return 0;
3352                 }
3353
3354                 if (skb_queue_is_last(&chan->srej_q, next_skb))
3355                         break;
3356
3357         } while ((next_skb = skb_queue_next(&chan->srej_q, next_skb)));
3358
3359         __skb_queue_tail(&chan->srej_q, skb);
3360
3361         return 0;
3362 }
3363
3364 static void append_skb_frag(struct sk_buff *skb,
3365                         struct sk_buff *new_frag, struct sk_buff **last_frag)
3366 {
3367         /* skb->len reflects data in skb as well as all fragments
3368          * skb->data_len reflects only data in fragments
3369          */
3370         if (!skb_has_frag_list(skb))
3371                 skb_shinfo(skb)->frag_list = new_frag;
3372
3373         new_frag->next = NULL;
3374
3375         (*last_frag)->next = new_frag;
3376         *last_frag = new_frag;
3377
3378         skb->len += new_frag->len;
3379         skb->data_len += new_frag->len;
3380         skb->truesize += new_frag->truesize;
3381 }
3382
3383 static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u32 control)
3384 {
3385         int err = -EINVAL;
3386
3387         switch (__get_ctrl_sar(chan, control)) {
3388         case L2CAP_SAR_UNSEGMENTED:
3389                 if (chan->sdu)
3390                         break;
3391
3392                 err = chan->ops->recv(chan->data, skb);
3393                 break;
3394
3395         case L2CAP_SAR_START:
3396                 if (chan->sdu)
3397                         break;
3398
3399                 chan->sdu_len = get_unaligned_le16(skb->data);
3400                 skb_pull(skb, L2CAP_SDULEN_SIZE);
3401
3402                 if (chan->sdu_len > chan->imtu) {
3403                         err = -EMSGSIZE;
3404                         break;
3405                 }
3406
3407                 if (skb->len >= chan->sdu_len)
3408                         break;
3409
3410                 chan->sdu = skb;
3411                 chan->sdu_last_frag = skb;
3412
3413                 skb = NULL;
3414                 err = 0;
3415                 break;
3416
3417         case L2CAP_SAR_CONTINUE:
3418                 if (!chan->sdu)
3419                         break;
3420
3421                 append_skb_frag(chan->sdu, skb,
3422                                 &chan->sdu_last_frag);
3423                 skb = NULL;
3424
3425                 if (chan->sdu->len >= chan->sdu_len)
3426                         break;
3427
3428                 err = 0;
3429                 break;
3430
3431         case L2CAP_SAR_END:
3432                 if (!chan->sdu)
3433                         break;
3434
3435                 append_skb_frag(chan->sdu, skb,
3436                                 &chan->sdu_last_frag);
3437                 skb = NULL;
3438
3439                 if (chan->sdu->len != chan->sdu_len)
3440                         break;
3441
3442                 err = chan->ops->recv(chan->data, chan->sdu);
3443
3444                 if (!err) {
3445                         /* Reassembly complete */
3446                         chan->sdu = NULL;
3447                         chan->sdu_last_frag = NULL;
3448                         chan->sdu_len = 0;
3449                 }
3450                 break;
3451         }
3452
3453         if (err) {
3454                 kfree_skb(skb);
3455                 kfree_skb(chan->sdu);
3456                 chan->sdu = NULL;
3457                 chan->sdu_last_frag = NULL;
3458                 chan->sdu_len = 0;
3459         }
3460
3461         return err;
3462 }
3463
3464 static void l2cap_ertm_enter_local_busy(struct l2cap_chan *chan)
3465 {
3466         u32 control;
3467
3468         BT_DBG("chan %p, Enter local busy", chan);
3469
3470         set_bit(CONN_LOCAL_BUSY, &chan->conn_state);
3471
3472         control = __set_reqseq(chan, chan->buffer_seq);
3473         control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR);
3474         l2cap_send_sframe(chan, control);
3475
3476         set_bit(CONN_RNR_SENT, &chan->conn_state);
3477
3478         __clear_ack_timer(chan);
3479 }
3480
3481 static void l2cap_ertm_exit_local_busy(struct l2cap_chan *chan)
3482 {
3483         u32 control;
3484
3485         if (!test_bit(CONN_RNR_SENT, &chan->conn_state))
3486                 goto done;
3487
3488         control = __set_reqseq(chan, chan->buffer_seq);
3489         control |= __set_ctrl_poll(chan);
3490         control |= __set_ctrl_super(chan, L2CAP_SUPER_RR);
3491         l2cap_send_sframe(chan, control);
3492         chan->retry_count = 1;
3493
3494         __clear_retrans_timer(chan);
3495         __set_monitor_timer(chan);
3496
3497         set_bit(CONN_WAIT_F, &chan->conn_state);
3498
3499 done:
3500         clear_bit(CONN_LOCAL_BUSY, &chan->conn_state);
3501         clear_bit(CONN_RNR_SENT, &chan->conn_state);
3502
3503         BT_DBG("chan %p, Exit local busy", chan);
3504 }
3505
3506 void l2cap_chan_busy(struct l2cap_chan *chan, int busy)
3507 {
3508         if (chan->mode == L2CAP_MODE_ERTM) {
3509                 if (busy)
3510                         l2cap_ertm_enter_local_busy(chan);
3511                 else
3512                         l2cap_ertm_exit_local_busy(chan);
3513         }
3514 }
3515
3516 static void l2cap_check_srej_gap(struct l2cap_chan *chan, u16 tx_seq)
3517 {
3518         struct sk_buff *skb;
3519         u32 control;
3520
3521         while ((skb = skb_peek(&chan->srej_q)) &&
3522                         !test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
3523                 int err;
3524
3525                 if (bt_cb(skb)->tx_seq != tx_seq)
3526                         break;
3527
3528                 skb = skb_dequeue(&chan->srej_q);
3529                 control = __set_ctrl_sar(chan, bt_cb(skb)->sar);
3530                 err = l2cap_reassemble_sdu(chan, skb, control);
3531
3532                 if (err < 0) {
3533                         l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3534                         break;
3535                 }
3536
3537                 chan->buffer_seq_srej = __next_seq(chan, chan->buffer_seq_srej);
3538                 tx_seq = __next_seq(chan, tx_seq);
3539         }
3540 }
3541
3542 static void l2cap_resend_srejframe(struct l2cap_chan *chan, u16 tx_seq)
3543 {
3544         struct srej_list *l, *tmp;
3545         u32 control;
3546
3547         list_for_each_entry_safe(l, tmp, &chan->srej_l, list) {
3548                 if (l->tx_seq == tx_seq) {
3549                         list_del(&l->list);
3550                         kfree(l);
3551                         return;
3552                 }
3553                 control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ);
3554                 control |= __set_reqseq(chan, l->tx_seq);
3555                 l2cap_send_sframe(chan, control);
3556                 list_del(&l->list);
3557                 list_add_tail(&l->list, &chan->srej_l);
3558         }
3559 }
3560
3561 static void l2cap_send_srejframe(struct l2cap_chan *chan, u16 tx_seq)
3562 {
3563         struct srej_list *new;
3564         u32 control;
3565
3566         while (tx_seq != chan->expected_tx_seq) {
3567                 control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ);
3568                 control |= __set_reqseq(chan, chan->expected_tx_seq);
3569                 l2cap_send_sframe(chan, control);
3570
3571                 new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC);
3572                 new->tx_seq = chan->expected_tx_seq;
3573
3574                 chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq);
3575
3576                 list_add_tail(&new->list, &chan->srej_l);
3577         }
3578
3579         chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq);
3580 }
3581
3582 static inline int l2cap_data_channel_iframe(struct l2cap_chan *chan, u32 rx_control, struct sk_buff *skb)
3583 {
3584         u16 tx_seq = __get_txseq(chan, rx_control);
3585         u16 req_seq = __get_reqseq(chan, rx_control);
3586         u8 sar = __get_ctrl_sar(chan, rx_control);
3587         int tx_seq_offset, expected_tx_seq_offset;
3588         int num_to_ack = (chan->tx_win/6) + 1;
3589         int err = 0;
3590
3591         BT_DBG("chan %p len %d tx_seq %d rx_control 0x%8.8x", chan, skb->len,
3592                                                         tx_seq, rx_control);
3593
3594         if (__is_ctrl_final(chan, rx_control) &&
3595                         test_bit(CONN_WAIT_F, &chan->conn_state)) {
3596                 __clear_monitor_timer(chan);
3597                 if (chan->unacked_frames > 0)
3598                         __set_retrans_timer(chan);
3599                 clear_bit(CONN_WAIT_F, &chan->conn_state);
3600         }
3601
3602         chan->expected_ack_seq = req_seq;
3603         l2cap_drop_acked_frames(chan);
3604
3605         tx_seq_offset = __seq_offset(chan, tx_seq, chan->buffer_seq);
3606
3607         /* invalid tx_seq */
3608         if (tx_seq_offset >= chan->tx_win) {
3609                 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3610                 goto drop;
3611         }
3612
3613         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state))
3614                 goto drop;
3615
3616         if (tx_seq == chan->expected_tx_seq)
3617                 goto expected;
3618
3619         if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
3620                 struct srej_list *first;
3621
3622                 first = list_first_entry(&chan->srej_l,
3623                                 struct srej_list, list);
3624                 if (tx_seq == first->tx_seq) {
3625                         l2cap_add_to_srej_queue(chan, skb, tx_seq, sar);
3626                         l2cap_check_srej_gap(chan, tx_seq);
3627
3628                         list_del(&first->list);
3629                         kfree(first);
3630
3631                         if (list_empty(&chan->srej_l)) {
3632                                 chan->buffer_seq = chan->buffer_seq_srej;
3633                                 clear_bit(CONN_SREJ_SENT, &chan->conn_state);
3634                                 l2cap_send_ack(chan);
3635                                 BT_DBG("chan %p, Exit SREJ_SENT", chan);
3636                         }
3637                 } else {
3638                         struct srej_list *l;
3639
3640                         /* duplicated tx_seq */
3641                         if (l2cap_add_to_srej_queue(chan, skb, tx_seq, sar) < 0)
3642                                 goto drop;
3643
3644                         list_for_each_entry(l, &chan->srej_l, list) {
3645                                 if (l->tx_seq == tx_seq) {
3646                                         l2cap_resend_srejframe(chan, tx_seq);
3647                                         return 0;
3648                                 }
3649                         }
3650                         l2cap_send_srejframe(chan, tx_seq);
3651                 }
3652         } else {
3653                 expected_tx_seq_offset = __seq_offset(chan,
3654                                 chan->expected_tx_seq, chan->buffer_seq);
3655
3656                 /* duplicated tx_seq */
3657                 if (tx_seq_offset < expected_tx_seq_offset)
3658                         goto drop;
3659
3660                 set_bit(CONN_SREJ_SENT, &chan->conn_state);
3661
3662                 BT_DBG("chan %p, Enter SREJ", chan);
3663
3664                 INIT_LIST_HEAD(&chan->srej_l);
3665                 chan->buffer_seq_srej = chan->buffer_seq;
3666
3667                 __skb_queue_head_init(&chan->srej_q);
3668                 l2cap_add_to_srej_queue(chan, skb, tx_seq, sar);
3669
3670                 set_bit(CONN_SEND_PBIT, &chan->conn_state);
3671
3672                 l2cap_send_srejframe(chan, tx_seq);
3673
3674                 __clear_ack_timer(chan);
3675         }
3676         return 0;
3677
3678 expected:
3679         chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq);
3680
3681         if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
3682                 bt_cb(skb)->tx_seq = tx_seq;
3683                 bt_cb(skb)->sar = sar;
3684                 __skb_queue_tail(&chan->srej_q, skb);
3685                 return 0;
3686         }
3687
3688         err = l2cap_reassemble_sdu(chan, skb, rx_control);
3689         chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
3690
3691         if (err < 0) {
3692                 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3693                 return err;
3694         }
3695
3696         if (__is_ctrl_final(chan, rx_control)) {
3697                 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
3698                         l2cap_retransmit_frames(chan);
3699         }
3700
3701         __set_ack_timer(chan);
3702
3703         chan->num_acked = (chan->num_acked + 1) % num_to_ack;
3704         if (chan->num_acked == num_to_ack - 1)
3705                 l2cap_send_ack(chan);
3706
3707         return 0;
3708
3709 drop:
3710         kfree_skb(skb);
3711         return 0;
3712 }
3713
3714 static inline void l2cap_data_channel_rrframe(struct l2cap_chan *chan, u32 rx_control)
3715 {
3716         BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan,
3717                                 __get_reqseq(chan, rx_control), rx_control);
3718
3719         chan->expected_ack_seq = __get_reqseq(chan, rx_control);
3720         l2cap_drop_acked_frames(chan);
3721
3722         if (__is_ctrl_poll(chan, rx_control)) {
3723                 set_bit(CONN_SEND_FBIT, &chan->conn_state);
3724                 if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
3725                         if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
3726                                         (chan->unacked_frames > 0))
3727                                 __set_retrans_timer(chan);
3728
3729                         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3730                         l2cap_send_srejtail(chan);
3731                 } else {
3732                         l2cap_send_i_or_rr_or_rnr(chan);
3733                 }
3734
3735         } else if (__is_ctrl_final(chan, rx_control)) {
3736                 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3737
3738                 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
3739                         l2cap_retransmit_frames(chan);
3740
3741         } else {
3742                 if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
3743                                 (chan->unacked_frames > 0))
3744                         __set_retrans_timer(chan);
3745
3746                 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3747                 if (test_bit(CONN_SREJ_SENT, &chan->conn_state))
3748                         l2cap_send_ack(chan);
3749                 else
3750                         l2cap_ertm_send(chan);
3751         }
3752 }
3753
3754 static inline void l2cap_data_channel_rejframe(struct l2cap_chan *chan, u32 rx_control)
3755 {
3756         u16 tx_seq = __get_reqseq(chan, rx_control);
3757
3758         BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control);
3759
3760         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3761
3762         chan->expected_ack_seq = tx_seq;
3763         l2cap_drop_acked_frames(chan);
3764
3765         if (__is_ctrl_final(chan, rx_control)) {
3766                 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
3767                         l2cap_retransmit_frames(chan);
3768         } else {
3769                 l2cap_retransmit_frames(chan);
3770
3771                 if (test_bit(CONN_WAIT_F, &chan->conn_state))
3772                         set_bit(CONN_REJ_ACT, &chan->conn_state);
3773         }
3774 }
3775 static inline void l2cap_data_channel_srejframe(struct l2cap_chan *chan, u32 rx_control)
3776 {
3777         u16 tx_seq = __get_reqseq(chan, rx_control);
3778
3779         BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control);
3780
3781         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3782
3783         if (__is_ctrl_poll(chan, rx_control)) {
3784                 chan->expected_ack_seq = tx_seq;
3785                 l2cap_drop_acked_frames(chan);
3786
3787                 set_bit(CONN_SEND_FBIT, &chan->conn_state);
3788                 l2cap_retransmit_one_frame(chan, tx_seq);
3789
3790                 l2cap_ertm_send(chan);
3791
3792                 if (test_bit(CONN_WAIT_F, &chan->conn_state)) {
3793                         chan->srej_save_reqseq = tx_seq;
3794                         set_bit(CONN_SREJ_ACT, &chan->conn_state);
3795                 }
3796         } else if (__is_ctrl_final(chan, rx_control)) {
3797                 if (test_bit(CONN_SREJ_ACT, &chan->conn_state) &&
3798                                 chan->srej_save_reqseq == tx_seq)
3799                         clear_bit(CONN_SREJ_ACT, &chan->conn_state);
3800                 else
3801                         l2cap_retransmit_one_frame(chan, tx_seq);
3802         } else {
3803                 l2cap_retransmit_one_frame(chan, tx_seq);
3804                 if (test_bit(CONN_WAIT_F, &chan->conn_state)) {
3805                         chan->srej_save_reqseq = tx_seq;
3806                         set_bit(CONN_SREJ_ACT, &chan->conn_state);
3807                 }
3808         }
3809 }
3810
3811 static inline void l2cap_data_channel_rnrframe(struct l2cap_chan *chan, u32 rx_control)
3812 {
3813         u16 tx_seq = __get_reqseq(chan, rx_control);
3814
3815         BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control);
3816
3817         set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3818         chan->expected_ack_seq = tx_seq;
3819         l2cap_drop_acked_frames(chan);
3820
3821         if (__is_ctrl_poll(chan, rx_control))
3822                 set_bit(CONN_SEND_FBIT, &chan->conn_state);
3823
3824         if (!test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
3825                 __clear_retrans_timer(chan);
3826                 if (__is_ctrl_poll(chan, rx_control))
3827                         l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_FINAL);
3828                 return;
3829         }
3830
3831         if (__is_ctrl_poll(chan, rx_control)) {
3832                 l2cap_send_srejtail(chan);
3833         } else {
3834                 rx_control = __set_ctrl_super(chan, L2CAP_SUPER_RR);
3835                 l2cap_send_sframe(chan, rx_control);
3836         }
3837 }
3838
3839 static inline int l2cap_data_channel_sframe(struct l2cap_chan *chan, u32 rx_control, struct sk_buff *skb)
3840 {
3841         BT_DBG("chan %p rx_control 0x%8.8x len %d", chan, rx_control, skb->len);
3842
3843         if (__is_ctrl_final(chan, rx_control) &&
3844                         test_bit(CONN_WAIT_F, &chan->conn_state)) {
3845                 __clear_monitor_timer(chan);
3846                 if (chan->unacked_frames > 0)
3847                         __set_retrans_timer(chan);
3848                 clear_bit(CONN_WAIT_F, &chan->conn_state);
3849         }
3850
3851         switch (__get_ctrl_super(chan, rx_control)) {
3852         case L2CAP_SUPER_RR:
3853                 l2cap_data_channel_rrframe(chan, rx_control);
3854                 break;
3855
3856         case L2CAP_SUPER_REJ:
3857                 l2cap_data_channel_rejframe(chan, rx_control);
3858                 break;
3859
3860         case L2CAP_SUPER_SREJ:
3861                 l2cap_data_channel_srejframe(chan, rx_control);
3862                 break;
3863
3864         case L2CAP_SUPER_RNR:
3865                 l2cap_data_channel_rnrframe(chan, rx_control);
3866                 break;
3867         }
3868
3869         kfree_skb(skb);
3870         return 0;
3871 }
3872
3873 static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb)
3874 {
3875         struct l2cap_chan *chan = l2cap_pi(sk)->chan;
3876         u32 control;
3877         u16 req_seq;
3878         int len, next_tx_seq_offset, req_seq_offset;
3879
3880         control = __get_control(chan, skb->data);
3881         skb_pull(skb, __ctrl_size(chan));
3882         len = skb->len;
3883
3884         /*
3885          * We can just drop the corrupted I-frame here.
3886          * Receiver will miss it and start proper recovery
3887          * procedures and ask retransmission.
3888          */
3889         if (l2cap_check_fcs(chan, skb))
3890                 goto drop;
3891
3892         if (__is_sar_start(chan, control) && !__is_sframe(chan, control))
3893                 len -= L2CAP_SDULEN_SIZE;
3894
3895         if (chan->fcs == L2CAP_FCS_CRC16)
3896                 len -= L2CAP_FCS_SIZE;
3897
3898         if (len > chan->mps) {
3899                 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3900                 goto drop;
3901         }
3902
3903         req_seq = __get_reqseq(chan, control);
3904
3905         req_seq_offset = __seq_offset(chan, req_seq, chan->expected_ack_seq);
3906
3907         next_tx_seq_offset = __seq_offset(chan, chan->next_tx_seq,
3908                                                 chan->expected_ack_seq);
3909
3910         /* check for invalid req-seq */
3911         if (req_seq_offset > next_tx_seq_offset) {
3912                 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3913                 goto drop;
3914         }
3915
3916         if (!__is_sframe(chan, control)) {
3917                 if (len < 0) {
3918                         l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3919                         goto drop;
3920                 }
3921
3922                 l2cap_data_channel_iframe(chan, control, skb);
3923         } else {
3924                 if (len != 0) {
3925                         BT_ERR("%d", len);
3926                         l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3927                         goto drop;
3928                 }
3929
3930                 l2cap_data_channel_sframe(chan, control, skb);
3931         }
3932
3933         return 0;
3934
3935 drop:
3936         kfree_skb(skb);
3937         return 0;
3938 }
3939
3940 static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
3941 {
3942         struct l2cap_chan *chan;
3943         struct sock *sk = NULL;
3944         u32 control;
3945         u16 tx_seq;
3946         int len;
3947
3948         chan = l2cap_get_chan_by_scid(conn, cid);
3949         if (!chan) {
3950                 BT_DBG("unknown cid 0x%4.4x", cid);
3951                 goto drop;
3952         }
3953
3954         sk = chan->sk;
3955
3956         BT_DBG("chan %p, len %d", chan, skb->len);
3957
3958         if (chan->state != BT_CONNECTED)
3959                 goto drop;
3960
3961         switch (chan->mode) {
3962         case L2CAP_MODE_BASIC:
3963                 /* If socket recv buffers overflows we drop data here
3964                  * which is *bad* because L2CAP has to be reliable.
3965                  * But we don't have any other choice. L2CAP doesn't
3966                  * provide flow control mechanism. */
3967
3968                 if (chan->imtu < skb->len)
3969                         goto drop;
3970
3971                 if (!chan->ops->recv(chan->data, skb))
3972                         goto done;
3973                 break;
3974
3975         case L2CAP_MODE_ERTM:
3976                 if (!sock_owned_by_user(sk)) {
3977                         l2cap_ertm_data_rcv(sk, skb);
3978                 } else {
3979                         if (sk_add_backlog(sk, skb))
3980                                 goto drop;
3981                 }
3982
3983                 goto done;
3984
3985         case L2CAP_MODE_STREAMING:
3986                 control = __get_control(chan, skb->data);
3987                 skb_pull(skb, __ctrl_size(chan));
3988                 len = skb->len;
3989
3990                 if (l2cap_check_fcs(chan, skb))
3991                         goto drop;
3992
3993                 if (__is_sar_start(chan, control))
3994                         len -= L2CAP_SDULEN_SIZE;
3995
3996                 if (chan->fcs == L2CAP_FCS_CRC16)
3997                         len -= L2CAP_FCS_SIZE;
3998
3999                 if (len > chan->mps || len < 0 || __is_sframe(chan, control))
4000                         goto drop;
4001
4002                 tx_seq = __get_txseq(chan, control);
4003
4004                 if (chan->expected_tx_seq != tx_seq) {
4005                         /* Frame(s) missing - must discard partial SDU */
4006                         kfree_skb(chan->sdu);
4007                         chan->sdu = NULL;
4008                         chan->sdu_last_frag = NULL;
4009                         chan->sdu_len = 0;
4010
4011                         /* TODO: Notify userland of missing data */
4012                 }
4013
4014                 chan->expected_tx_seq = __next_seq(chan, tx_seq);
4015
4016                 if (l2cap_reassemble_sdu(chan, skb, control) == -EMSGSIZE)
4017                         l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
4018
4019                 goto done;
4020
4021         default:
4022                 BT_DBG("chan %p: bad mode 0x%2.2x", chan, chan->mode);
4023                 break;
4024         }
4025
4026 drop:
4027         kfree_skb(skb);
4028
4029 done:
4030         if (sk)
4031                 bh_unlock_sock(sk);
4032
4033         return 0;
4034 }
4035
4036 static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb)
4037 {
4038         struct sock *sk = NULL;
4039         struct l2cap_chan *chan;
4040
4041         chan = l2cap_global_chan_by_psm(0, psm, conn->src);
4042         if (!chan)
4043                 goto drop;
4044
4045         sk = chan->sk;
4046
4047         bh_lock_sock(sk);
4048
4049         BT_DBG("sk %p, len %d", sk, skb->len);
4050
4051         if (chan->state != BT_BOUND && chan->state != BT_CONNECTED)
4052                 goto drop;
4053
4054         if (chan->imtu < skb->len)
4055                 goto drop;
4056
4057         if (!chan->ops->recv(chan->data, skb))
4058                 goto done;
4059
4060 drop:
4061         kfree_skb(skb);
4062
4063 done:
4064         if (sk)
4065                 bh_unlock_sock(sk);
4066         return 0;
4067 }
4068
4069 static inline int l2cap_att_channel(struct l2cap_conn *conn, __le16 cid, struct sk_buff *skb)
4070 {
4071         struct sock *sk = NULL;
4072         struct l2cap_chan *chan;
4073
4074         chan = l2cap_global_chan_by_scid(0, cid, conn->src);
4075         if (!chan)
4076                 goto drop;
4077
4078         sk = chan->sk;
4079
4080         bh_lock_sock(sk);
4081
4082         BT_DBG("sk %p, len %d", sk, skb->len);
4083
4084         if (chan->state != BT_BOUND && chan->state != BT_CONNECTED)
4085                 goto drop;
4086
4087         if (chan->imtu < skb->len)
4088                 goto drop;
4089
4090         if (!chan->ops->recv(chan->data, skb))
4091                 goto done;
4092
4093 drop:
4094         kfree_skb(skb);
4095
4096 done:
4097         if (sk)
4098                 bh_unlock_sock(sk);
4099         return 0;
4100 }
4101
4102 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
4103 {
4104         struct l2cap_hdr *lh = (void *) skb->data;
4105         u16 cid, len;
4106         __le16 psm;
4107
4108         skb_pull(skb, L2CAP_HDR_SIZE);
4109         cid = __le16_to_cpu(lh->cid);
4110         len = __le16_to_cpu(lh->len);
4111
4112         if (len != skb->len) {
4113                 kfree_skb(skb);
4114                 return;
4115         }
4116
4117         BT_DBG("len %d, cid 0x%4.4x", len, cid);
4118
4119         switch (cid) {
4120         case L2CAP_CID_LE_SIGNALING:
4121         case L2CAP_CID_SIGNALING:
4122                 l2cap_sig_channel(conn, skb);
4123                 break;
4124
4125         case L2CAP_CID_CONN_LESS:
4126                 psm = get_unaligned_le16(skb->data);
4127                 skb_pull(skb, 2);
4128                 l2cap_conless_channel(conn, psm, skb);
4129                 break;
4130
4131         case L2CAP_CID_LE_DATA:
4132                 l2cap_att_channel(conn, cid, skb);
4133                 break;
4134
4135         case L2CAP_CID_SMP:
4136                 if (smp_sig_channel(conn, skb))
4137                         l2cap_conn_del(conn->hcon, EACCES);
4138                 break;
4139
4140         default:
4141                 l2cap_data_channel(conn, cid, skb);
4142                 break;
4143         }
4144 }
4145
4146 /* ---- L2CAP interface with lower layer (HCI) ---- */
4147
4148 static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
4149 {
4150         int exact = 0, lm1 = 0, lm2 = 0;
4151         struct l2cap_chan *c;
4152
4153         if (type != ACL_LINK)
4154                 return -EINVAL;
4155
4156         BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
4157
4158         /* Find listening sockets and check their link_mode */
4159         read_lock(&chan_list_lock);
4160         list_for_each_entry(c, &chan_list, global_l) {
4161                 struct sock *sk = c->sk;
4162
4163                 if (c->state != BT_LISTEN)
4164                         continue;
4165
4166                 if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
4167                         lm1 |= HCI_LM_ACCEPT;
4168                         if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
4169                                 lm1 |= HCI_LM_MASTER;
4170                         exact++;
4171                 } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) {
4172                         lm2 |= HCI_LM_ACCEPT;
4173                         if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
4174                                 lm2 |= HCI_LM_MASTER;
4175                 }
4176         }
4177         read_unlock(&chan_list_lock);
4178
4179         return exact ? lm1 : lm2;
4180 }
4181
4182 static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
4183 {
4184         struct l2cap_conn *conn;
4185
4186         BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
4187
4188         if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK))
4189                 return -EINVAL;
4190
4191         if (!status) {
4192                 conn = l2cap_conn_add(hcon, status);
4193                 if (conn)
4194                         l2cap_conn_ready(conn);
4195         } else
4196                 l2cap_conn_del(hcon, bt_to_errno(status));
4197
4198         return 0;
4199 }
4200
4201 static int l2cap_disconn_ind(struct hci_conn *hcon)
4202 {
4203         struct l2cap_conn *conn = hcon->l2cap_data;
4204
4205         BT_DBG("hcon %p", hcon);
4206
4207         if ((hcon->type != ACL_LINK && hcon->type != LE_LINK) || !conn)
4208                 return 0x13;
4209
4210         return conn->disc_reason;
4211 }
4212
4213 static int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
4214 {
4215         BT_DBG("hcon %p reason %d", hcon, reason);
4216
4217         if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK))
4218                 return -EINVAL;
4219
4220         l2cap_conn_del(hcon, bt_to_errno(reason));
4221
4222         return 0;
4223 }
4224
4225 static inline void l2cap_check_encryption(struct l2cap_chan *chan, u8 encrypt)
4226 {
4227         if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED)
4228                 return;
4229
4230         if (encrypt == 0x00) {
4231                 if (chan->sec_level == BT_SECURITY_MEDIUM) {
4232                         __clear_chan_timer(chan);
4233                         __set_chan_timer(chan, HZ * 5);
4234                 } else if (chan->sec_level == BT_SECURITY_HIGH)
4235                         l2cap_chan_close(chan, ECONNREFUSED);
4236         } else {
4237                 if (chan->sec_level == BT_SECURITY_MEDIUM)
4238                         __clear_chan_timer(chan);
4239         }
4240 }
4241
4242 static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
4243 {
4244         struct l2cap_conn *conn = hcon->l2cap_data;
4245         struct l2cap_chan *chan;
4246
4247         if (!conn)
4248                 return 0;
4249
4250         BT_DBG("conn %p", conn);
4251
4252         if (hcon->type == LE_LINK) {
4253                 smp_distribute_keys(conn, 0);
4254                 del_timer(&conn->security_timer);
4255         }
4256
4257         read_lock(&conn->chan_lock);
4258
4259         list_for_each_entry(chan, &conn->chan_l, list) {
4260                 struct sock *sk = chan->sk;
4261
4262                 bh_lock_sock(sk);
4263
4264                 BT_DBG("chan->scid %d", chan->scid);
4265
4266                 if (chan->scid == L2CAP_CID_LE_DATA) {
4267                         if (!status && encrypt) {
4268                                 chan->sec_level = hcon->sec_level;
4269                                 l2cap_chan_ready(sk);
4270                         }
4271
4272                         bh_unlock_sock(sk);
4273                         continue;
4274                 }
4275
4276                 if (test_bit(CONF_CONNECT_PEND, &chan->conf_state)) {
4277                         bh_unlock_sock(sk);
4278                         continue;
4279                 }
4280
4281                 if (!status && (chan->state == BT_CONNECTED ||
4282                                                 chan->state == BT_CONFIG)) {
4283                         l2cap_check_encryption(chan, encrypt);
4284                         bh_unlock_sock(sk);
4285                         continue;
4286                 }
4287
4288                 if (chan->state == BT_CONNECT) {
4289                         if (!status) {
4290                                 struct l2cap_conn_req req;
4291                                 req.scid = cpu_to_le16(chan->scid);
4292                                 req.psm  = chan->psm;
4293
4294                                 chan->ident = l2cap_get_ident(conn);
4295                                 set_bit(CONF_CONNECT_PEND, &chan->conf_state);
4296
4297                                 l2cap_send_cmd(conn, chan->ident,
4298                                         L2CAP_CONN_REQ, sizeof(req), &req);
4299                         } else {
4300                                 __clear_chan_timer(chan);
4301                                 __set_chan_timer(chan, HZ / 10);
4302                         }
4303                 } else if (chan->state == BT_CONNECT2) {
4304                         struct l2cap_conn_rsp rsp;
4305                         __u16 res, stat;
4306
4307                         if (!status) {
4308                                 if (bt_sk(sk)->defer_setup) {
4309                                         struct sock *parent = bt_sk(sk)->parent;
4310                                         res = L2CAP_CR_PEND;
4311                                         stat = L2CAP_CS_AUTHOR_PEND;
4312                                         if (parent)
4313                                                 parent->sk_data_ready(parent, 0);
4314                                 } else {
4315                                         l2cap_state_change(chan, BT_CONFIG);
4316                                         res = L2CAP_CR_SUCCESS;
4317                                         stat = L2CAP_CS_NO_INFO;
4318                                 }
4319                         } else {
4320                                 l2cap_state_change(chan, BT_DISCONN);
4321                                 __set_chan_timer(chan, HZ / 10);
4322                                 res = L2CAP_CR_SEC_BLOCK;
4323                                 stat = L2CAP_CS_NO_INFO;
4324                         }
4325
4326                         rsp.scid   = cpu_to_le16(chan->dcid);
4327                         rsp.dcid   = cpu_to_le16(chan->scid);
4328                         rsp.result = cpu_to_le16(res);
4329                         rsp.status = cpu_to_le16(stat);
4330                         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
4331                                                         sizeof(rsp), &rsp);
4332                 }
4333
4334                 bh_unlock_sock(sk);
4335         }
4336
4337         read_unlock(&conn->chan_lock);
4338
4339         return 0;
4340 }
4341
4342 static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
4343 {
4344         struct l2cap_conn *conn = hcon->l2cap_data;
4345
4346         if (!conn)
4347                 conn = l2cap_conn_add(hcon, 0);
4348
4349         if (!conn)
4350                 goto drop;
4351
4352         BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
4353
4354         if (!(flags & ACL_CONT)) {
4355                 struct l2cap_hdr *hdr;
4356                 struct l2cap_chan *chan;
4357                 u16 cid;
4358                 int len;
4359
4360                 if (conn->rx_len) {
4361                         BT_ERR("Unexpected start frame (len %d)", skb->len);
4362                         kfree_skb(conn->rx_skb);
4363                         conn->rx_skb = NULL;
4364                         conn->rx_len = 0;
4365                         l2cap_conn_unreliable(conn, ECOMM);
4366                 }
4367
4368                 /* Start fragment always begin with Basic L2CAP header */
4369                 if (skb->len < L2CAP_HDR_SIZE) {
4370                         BT_ERR("Frame is too short (len %d)", skb->len);
4371                         l2cap_conn_unreliable(conn, ECOMM);
4372                         goto drop;
4373                 }
4374
4375                 hdr = (struct l2cap_hdr *) skb->data;
4376                 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
4377                 cid = __le16_to_cpu(hdr->cid);
4378
4379                 if (len == skb->len) {
4380                         /* Complete frame received */
4381                         l2cap_recv_frame(conn, skb);
4382                         return 0;
4383                 }
4384
4385                 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
4386
4387                 if (skb->len > len) {
4388                         BT_ERR("Frame is too long (len %d, expected len %d)",
4389                                 skb->len, len);
4390                         l2cap_conn_unreliable(conn, ECOMM);
4391                         goto drop;
4392                 }
4393
4394                 chan = l2cap_get_chan_by_scid(conn, cid);
4395
4396                 if (chan && chan->sk) {
4397                         struct sock *sk = chan->sk;
4398
4399                         if (chan->imtu < len - L2CAP_HDR_SIZE) {
4400                                 BT_ERR("Frame exceeding recv MTU (len %d, "
4401                                                         "MTU %d)", len,
4402                                                         chan->imtu);
4403                                 bh_unlock_sock(sk);
4404                                 l2cap_conn_unreliable(conn, ECOMM);
4405                                 goto drop;
4406                         }
4407                         bh_unlock_sock(sk);
4408                 }
4409
4410                 /* Allocate skb for the complete frame (with header) */
4411                 conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC);
4412                 if (!conn->rx_skb)
4413                         goto drop;
4414
4415                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
4416                                                                 skb->len);
4417                 conn->rx_len = len - skb->len;
4418         } else {
4419                 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
4420
4421                 if (!conn->rx_len) {
4422                         BT_ERR("Unexpected continuation frame (len %d)", skb->len);
4423                         l2cap_conn_unreliable(conn, ECOMM);
4424                         goto drop;
4425                 }
4426
4427                 if (skb->len > conn->rx_len) {
4428                         BT_ERR("Fragment is too long (len %d, expected %d)",
4429                                         skb->len, conn->rx_len);
4430                         kfree_skb(conn->rx_skb);
4431                         conn->rx_skb = NULL;
4432                         conn->rx_len = 0;
4433                         l2cap_conn_unreliable(conn, ECOMM);
4434                         goto drop;
4435                 }
4436
4437                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
4438                                                                 skb->len);
4439                 conn->rx_len -= skb->len;
4440
4441                 if (!conn->rx_len) {
4442                         /* Complete frame received */
4443                         l2cap_recv_frame(conn, conn->rx_skb);
4444                         conn->rx_skb = NULL;
4445                 }
4446         }
4447
4448 drop:
4449         kfree_skb(skb);
4450         return 0;
4451 }
4452
4453 static int l2cap_debugfs_show(struct seq_file *f, void *p)
4454 {
4455         struct l2cap_chan *c;
4456
4457         read_lock_bh(&chan_list_lock);
4458
4459         list_for_each_entry(c, &chan_list, global_l) {
4460                 struct sock *sk = c->sk;
4461
4462                 seq_printf(f, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n",
4463                                         batostr(&bt_sk(sk)->src),
4464                                         batostr(&bt_sk(sk)->dst),
4465                                         c->state, __le16_to_cpu(c->psm),
4466                                         c->scid, c->dcid, c->imtu, c->omtu,
4467                                         c->sec_level, c->mode);
4468 }
4469
4470         read_unlock_bh(&chan_list_lock);
4471
4472         return 0;
4473 }
4474
4475 static int l2cap_debugfs_open(struct inode *inode, struct file *file)
4476 {
4477         return single_open(file, l2cap_debugfs_show, inode->i_private);
4478 }
4479
4480 static const struct file_operations l2cap_debugfs_fops = {
4481         .open           = l2cap_debugfs_open,
4482         .read           = seq_read,
4483         .llseek         = seq_lseek,
4484         .release        = single_release,
4485 };
4486
4487 static struct dentry *l2cap_debugfs;
4488
4489 static struct hci_proto l2cap_hci_proto = {
4490         .name           = "L2CAP",
4491         .id             = HCI_PROTO_L2CAP,
4492         .connect_ind    = l2cap_connect_ind,
4493         .connect_cfm    = l2cap_connect_cfm,
4494         .disconn_ind    = l2cap_disconn_ind,
4495         .disconn_cfm    = l2cap_disconn_cfm,
4496         .security_cfm   = l2cap_security_cfm,
4497         .recv_acldata   = l2cap_recv_acldata
4498 };
4499
4500 int __init l2cap_init(void)
4501 {
4502         int err;
4503
4504         err = l2cap_init_sockets();
4505         if (err < 0)
4506                 return err;
4507
4508         err = hci_register_proto(&l2cap_hci_proto);
4509         if (err < 0) {
4510                 BT_ERR("L2CAP protocol registration failed");
4511                 bt_sock_unregister(BTPROTO_L2CAP);
4512                 goto error;
4513         }
4514
4515         if (bt_debugfs) {
4516                 l2cap_debugfs = debugfs_create_file("l2cap", 0444,
4517                                         bt_debugfs, NULL, &l2cap_debugfs_fops);
4518                 if (!l2cap_debugfs)
4519                         BT_ERR("Failed to create L2CAP debug file");
4520         }
4521
4522         return 0;
4523
4524 error:
4525         l2cap_cleanup_sockets();
4526         return err;
4527 }
4528
4529 void l2cap_exit(void)
4530 {
4531         debugfs_remove(l2cap_debugfs);
4532
4533         if (hci_unregister_proto(&l2cap_hci_proto) < 0)
4534                 BT_ERR("L2CAP protocol unregistration failed");
4535
4536         l2cap_cleanup_sockets();
4537 }
4538
4539 module_param(disable_ertm, bool, 0644);
4540 MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode");
4541
4542 module_param(enable_hs, bool, 0644);
4543 MODULE_PARM_DESC(enable_hs, "Enable High Speed");