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