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