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