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