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