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