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