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