Bluetooth: Move code for ERTM local busy state to separate functions
[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         if (err >= 0) {
3354                 chan->buffer_seq = (chan->buffer_seq + 1) % 64;
3355                 return err;
3356         }
3357
3358         l2cap_ertm_enter_local_busy(chan);
3359
3360         bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT;
3361         __skb_queue_tail(&chan->busy_q, skb);
3362
3363         queue_work(_busy_wq, &chan->busy_work);
3364
3365         return err;
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                 if (bt_cb(skb)->tx_seq != tx_seq)
3468                         break;
3469
3470                 skb = skb_dequeue(&chan->srej_q);
3471                 control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
3472                 l2cap_ertm_reassembly_sdu(chan, skb, control);
3473                 chan->buffer_seq_srej =
3474                         (chan->buffer_seq_srej + 1) % 64;
3475                 tx_seq = (tx_seq + 1) % 64;
3476         }
3477 }
3478
3479 static void l2cap_resend_srejframe(struct l2cap_chan *chan, u8 tx_seq)
3480 {
3481         struct srej_list *l, *tmp;
3482         u16 control;
3483
3484         list_for_each_entry_safe(l, tmp, &chan->srej_l, list) {
3485                 if (l->tx_seq == tx_seq) {
3486                         list_del(&l->list);
3487                         kfree(l);
3488                         return;
3489                 }
3490                 control = L2CAP_SUPER_SELECT_REJECT;
3491                 control |= l->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3492                 l2cap_send_sframe(chan, control);
3493                 list_del(&l->list);
3494                 list_add_tail(&l->list, &chan->srej_l);
3495         }
3496 }
3497
3498 static void l2cap_send_srejframe(struct l2cap_chan *chan, u8 tx_seq)
3499 {
3500         struct srej_list *new;
3501         u16 control;
3502
3503         while (tx_seq != chan->expected_tx_seq) {
3504                 control = L2CAP_SUPER_SELECT_REJECT;
3505                 control |= chan->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3506                 l2cap_send_sframe(chan, control);
3507
3508                 new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC);
3509                 new->tx_seq = chan->expected_tx_seq;
3510                 chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64;
3511                 list_add_tail(&new->list, &chan->srej_l);
3512         }
3513         chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64;
3514 }
3515
3516 static inline int l2cap_data_channel_iframe(struct l2cap_chan *chan, u16 rx_control, struct sk_buff *skb)
3517 {
3518         u8 tx_seq = __get_txseq(rx_control);
3519         u8 req_seq = __get_reqseq(rx_control);
3520         u8 sar = rx_control >> L2CAP_CTRL_SAR_SHIFT;
3521         int tx_seq_offset, expected_tx_seq_offset;
3522         int num_to_ack = (chan->tx_win/6) + 1;
3523         int err = 0;
3524
3525         BT_DBG("chan %p len %d tx_seq %d rx_control 0x%4.4x", chan, skb->len,
3526                                                         tx_seq, rx_control);
3527
3528         if (L2CAP_CTRL_FINAL & rx_control &&
3529                         test_bit(CONN_WAIT_F, &chan->conn_state)) {
3530                 __clear_monitor_timer(chan);
3531                 if (chan->unacked_frames > 0)
3532                         __set_retrans_timer(chan);
3533                 clear_bit(CONN_WAIT_F, &chan->conn_state);
3534         }
3535
3536         chan->expected_ack_seq = req_seq;
3537         l2cap_drop_acked_frames(chan);
3538
3539         tx_seq_offset = (tx_seq - chan->buffer_seq) % 64;
3540         if (tx_seq_offset < 0)
3541                 tx_seq_offset += 64;
3542
3543         /* invalid tx_seq */
3544         if (tx_seq_offset >= chan->tx_win) {
3545                 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3546                 goto drop;
3547         }
3548
3549         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state))
3550                 goto drop;
3551
3552         if (tx_seq == chan->expected_tx_seq)
3553                 goto expected;
3554
3555         if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
3556                 struct srej_list *first;
3557
3558                 first = list_first_entry(&chan->srej_l,
3559                                 struct srej_list, list);
3560                 if (tx_seq == first->tx_seq) {
3561                         l2cap_add_to_srej_queue(chan, skb, tx_seq, sar);
3562                         l2cap_check_srej_gap(chan, tx_seq);
3563
3564                         list_del(&first->list);
3565                         kfree(first);
3566
3567                         if (list_empty(&chan->srej_l)) {
3568                                 chan->buffer_seq = chan->buffer_seq_srej;
3569                                 clear_bit(CONN_SREJ_SENT, &chan->conn_state);
3570                                 l2cap_send_ack(chan);
3571                                 BT_DBG("chan %p, Exit SREJ_SENT", chan);
3572                         }
3573                 } else {
3574                         struct srej_list *l;
3575
3576                         /* duplicated tx_seq */
3577                         if (l2cap_add_to_srej_queue(chan, skb, tx_seq, sar) < 0)
3578                                 goto drop;
3579
3580                         list_for_each_entry(l, &chan->srej_l, list) {
3581                                 if (l->tx_seq == tx_seq) {
3582                                         l2cap_resend_srejframe(chan, tx_seq);
3583                                         return 0;
3584                                 }
3585                         }
3586                         l2cap_send_srejframe(chan, tx_seq);
3587                 }
3588         } else {
3589                 expected_tx_seq_offset =
3590                         (chan->expected_tx_seq - chan->buffer_seq) % 64;
3591                 if (expected_tx_seq_offset < 0)
3592                         expected_tx_seq_offset += 64;
3593
3594                 /* duplicated tx_seq */
3595                 if (tx_seq_offset < expected_tx_seq_offset)
3596                         goto drop;
3597
3598                 set_bit(CONN_SREJ_SENT, &chan->conn_state);
3599
3600                 BT_DBG("chan %p, Enter SREJ", chan);
3601
3602                 INIT_LIST_HEAD(&chan->srej_l);
3603                 chan->buffer_seq_srej = chan->buffer_seq;
3604
3605                 __skb_queue_head_init(&chan->srej_q);
3606                 __skb_queue_head_init(&chan->busy_q);
3607                 l2cap_add_to_srej_queue(chan, skb, tx_seq, sar);
3608
3609                 set_bit(CONN_SEND_PBIT, &chan->conn_state);
3610
3611                 l2cap_send_srejframe(chan, tx_seq);
3612
3613                 __clear_ack_timer(chan);
3614         }
3615         return 0;
3616
3617 expected:
3618         chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64;
3619
3620         if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
3621                 bt_cb(skb)->tx_seq = tx_seq;
3622                 bt_cb(skb)->sar = sar;
3623                 __skb_queue_tail(&chan->srej_q, skb);
3624                 return 0;
3625         }
3626
3627         err = l2cap_push_rx_skb(chan, skb, rx_control);
3628         if (err < 0)
3629                 return 0;
3630
3631         if (rx_control & L2CAP_CTRL_FINAL) {
3632                 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
3633                         l2cap_retransmit_frames(chan);
3634         }
3635
3636         __set_ack_timer(chan);
3637
3638         chan->num_acked = (chan->num_acked + 1) % num_to_ack;
3639         if (chan->num_acked == num_to_ack - 1)
3640                 l2cap_send_ack(chan);
3641
3642         return 0;
3643
3644 drop:
3645         kfree_skb(skb);
3646         return 0;
3647 }
3648
3649 static inline void l2cap_data_channel_rrframe(struct l2cap_chan *chan, u16 rx_control)
3650 {
3651         BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, __get_reqseq(rx_control),
3652                                                 rx_control);
3653
3654         chan->expected_ack_seq = __get_reqseq(rx_control);
3655         l2cap_drop_acked_frames(chan);
3656
3657         if (rx_control & L2CAP_CTRL_POLL) {
3658                 set_bit(CONN_SEND_FBIT, &chan->conn_state);
3659                 if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
3660                         if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
3661                                         (chan->unacked_frames > 0))
3662                                 __set_retrans_timer(chan);
3663
3664                         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3665                         l2cap_send_srejtail(chan);
3666                 } else {
3667                         l2cap_send_i_or_rr_or_rnr(chan);
3668                 }
3669
3670         } else if (rx_control & L2CAP_CTRL_FINAL) {
3671                 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3672
3673                 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
3674                         l2cap_retransmit_frames(chan);
3675
3676         } else {
3677                 if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
3678                                 (chan->unacked_frames > 0))
3679                         __set_retrans_timer(chan);
3680
3681                 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3682                 if (test_bit(CONN_SREJ_SENT, &chan->conn_state))
3683                         l2cap_send_ack(chan);
3684                 else
3685                         l2cap_ertm_send(chan);
3686         }
3687 }
3688
3689 static inline void l2cap_data_channel_rejframe(struct l2cap_chan *chan, u16 rx_control)
3690 {
3691         u8 tx_seq = __get_reqseq(rx_control);
3692
3693         BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, tx_seq, rx_control);
3694
3695         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3696
3697         chan->expected_ack_seq = tx_seq;
3698         l2cap_drop_acked_frames(chan);
3699
3700         if (rx_control & L2CAP_CTRL_FINAL) {
3701                 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
3702                         l2cap_retransmit_frames(chan);
3703         } else {
3704                 l2cap_retransmit_frames(chan);
3705
3706                 if (test_bit(CONN_WAIT_F, &chan->conn_state))
3707                         set_bit(CONN_REJ_ACT, &chan->conn_state);
3708         }
3709 }
3710 static inline void l2cap_data_channel_srejframe(struct l2cap_chan *chan, u16 rx_control)
3711 {
3712         u8 tx_seq = __get_reqseq(rx_control);
3713
3714         BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, tx_seq, rx_control);
3715
3716         clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3717
3718         if (rx_control & L2CAP_CTRL_POLL) {
3719                 chan->expected_ack_seq = tx_seq;
3720                 l2cap_drop_acked_frames(chan);
3721
3722                 set_bit(CONN_SEND_FBIT, &chan->conn_state);
3723                 l2cap_retransmit_one_frame(chan, tx_seq);
3724
3725                 l2cap_ertm_send(chan);
3726
3727                 if (test_bit(CONN_WAIT_F, &chan->conn_state)) {
3728                         chan->srej_save_reqseq = tx_seq;
3729                         set_bit(CONN_SREJ_ACT, &chan->conn_state);
3730                 }
3731         } else if (rx_control & L2CAP_CTRL_FINAL) {
3732                 if (test_bit(CONN_SREJ_ACT, &chan->conn_state) &&
3733                                 chan->srej_save_reqseq == tx_seq)
3734                         clear_bit(CONN_SREJ_ACT, &chan->conn_state);
3735                 else
3736                         l2cap_retransmit_one_frame(chan, tx_seq);
3737         } else {
3738                 l2cap_retransmit_one_frame(chan, tx_seq);
3739                 if (test_bit(CONN_WAIT_F, &chan->conn_state)) {
3740                         chan->srej_save_reqseq = tx_seq;
3741                         set_bit(CONN_SREJ_ACT, &chan->conn_state);
3742                 }
3743         }
3744 }
3745
3746 static inline void l2cap_data_channel_rnrframe(struct l2cap_chan *chan, u16 rx_control)
3747 {
3748         u8 tx_seq = __get_reqseq(rx_control);
3749
3750         BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, tx_seq, rx_control);
3751
3752         set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3753         chan->expected_ack_seq = tx_seq;
3754         l2cap_drop_acked_frames(chan);
3755
3756         if (rx_control & L2CAP_CTRL_POLL)
3757                 set_bit(CONN_SEND_FBIT, &chan->conn_state);
3758
3759         if (!test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
3760                 __clear_retrans_timer(chan);
3761                 if (rx_control & L2CAP_CTRL_POLL)
3762                         l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_FINAL);
3763                 return;
3764         }
3765
3766         if (rx_control & L2CAP_CTRL_POLL)
3767                 l2cap_send_srejtail(chan);
3768         else
3769                 l2cap_send_sframe(chan, L2CAP_SUPER_RCV_READY);
3770 }
3771
3772 static inline int l2cap_data_channel_sframe(struct l2cap_chan *chan, u16 rx_control, struct sk_buff *skb)
3773 {
3774         BT_DBG("chan %p rx_control 0x%4.4x len %d", chan, rx_control, skb->len);
3775
3776         if (L2CAP_CTRL_FINAL & rx_control &&
3777                         test_bit(CONN_WAIT_F, &chan->conn_state)) {
3778                 __clear_monitor_timer(chan);
3779                 if (chan->unacked_frames > 0)
3780                         __set_retrans_timer(chan);
3781                 clear_bit(CONN_WAIT_F, &chan->conn_state);
3782         }
3783
3784         switch (rx_control & L2CAP_CTRL_SUPERVISE) {
3785         case L2CAP_SUPER_RCV_READY:
3786                 l2cap_data_channel_rrframe(chan, rx_control);
3787                 break;
3788
3789         case L2CAP_SUPER_REJECT:
3790                 l2cap_data_channel_rejframe(chan, rx_control);
3791                 break;
3792
3793         case L2CAP_SUPER_SELECT_REJECT:
3794                 l2cap_data_channel_srejframe(chan, rx_control);
3795                 break;
3796
3797         case L2CAP_SUPER_RCV_NOT_READY:
3798                 l2cap_data_channel_rnrframe(chan, rx_control);
3799                 break;
3800         }
3801
3802         kfree_skb(skb);
3803         return 0;
3804 }
3805
3806 static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb)
3807 {
3808         struct l2cap_chan *chan = l2cap_pi(sk)->chan;
3809         u16 control;
3810         u8 req_seq;
3811         int len, next_tx_seq_offset, req_seq_offset;
3812
3813         control = get_unaligned_le16(skb->data);
3814         skb_pull(skb, 2);
3815         len = skb->len;
3816
3817         /*
3818          * We can just drop the corrupted I-frame here.
3819          * Receiver will miss it and start proper recovery
3820          * procedures and ask retransmission.
3821          */
3822         if (l2cap_check_fcs(chan, skb))
3823                 goto drop;
3824
3825         if (__is_sar_start(control) && __is_iframe(control))
3826                 len -= 2;
3827
3828         if (chan->fcs == L2CAP_FCS_CRC16)
3829                 len -= 2;
3830
3831         if (len > chan->mps) {
3832                 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3833                 goto drop;
3834         }
3835
3836         req_seq = __get_reqseq(control);
3837         req_seq_offset = (req_seq - chan->expected_ack_seq) % 64;
3838         if (req_seq_offset < 0)
3839                 req_seq_offset += 64;
3840
3841         next_tx_seq_offset =
3842                 (chan->next_tx_seq - chan->expected_ack_seq) % 64;
3843         if (next_tx_seq_offset < 0)
3844                 next_tx_seq_offset += 64;
3845
3846         /* check for invalid req-seq */
3847         if (req_seq_offset > next_tx_seq_offset) {
3848                 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3849                 goto drop;
3850         }
3851
3852         if (__is_iframe(control)) {
3853                 if (len < 0) {
3854                         l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3855                         goto drop;
3856                 }
3857
3858                 l2cap_data_channel_iframe(chan, control, skb);
3859         } else {
3860                 if (len != 0) {
3861                         BT_ERR("%d", len);
3862                         l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3863                         goto drop;
3864                 }
3865
3866                 l2cap_data_channel_sframe(chan, control, skb);
3867         }
3868
3869         return 0;
3870
3871 drop:
3872         kfree_skb(skb);
3873         return 0;
3874 }
3875
3876 static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
3877 {
3878         struct l2cap_chan *chan;
3879         struct sock *sk = NULL;
3880         u16 control;
3881         u8 tx_seq;
3882         int len;
3883
3884         chan = l2cap_get_chan_by_scid(conn, cid);
3885         if (!chan) {
3886                 BT_DBG("unknown cid 0x%4.4x", cid);
3887                 goto drop;
3888         }
3889
3890         sk = chan->sk;
3891
3892         BT_DBG("chan %p, len %d", chan, skb->len);
3893
3894         if (chan->state != BT_CONNECTED)
3895                 goto drop;
3896
3897         switch (chan->mode) {
3898         case L2CAP_MODE_BASIC:
3899                 /* If socket recv buffers overflows we drop data here
3900                  * which is *bad* because L2CAP has to be reliable.
3901                  * But we don't have any other choice. L2CAP doesn't
3902                  * provide flow control mechanism. */
3903
3904                 if (chan->imtu < skb->len)
3905                         goto drop;
3906
3907                 if (!chan->ops->recv(chan->data, skb))
3908                         goto done;
3909                 break;
3910
3911         case L2CAP_MODE_ERTM:
3912                 if (!sock_owned_by_user(sk)) {
3913                         l2cap_ertm_data_rcv(sk, skb);
3914                 } else {
3915                         if (sk_add_backlog(sk, skb))
3916                                 goto drop;
3917                 }
3918
3919                 goto done;
3920
3921         case L2CAP_MODE_STREAMING:
3922                 control = get_unaligned_le16(skb->data);
3923                 skb_pull(skb, 2);
3924                 len = skb->len;
3925
3926                 if (l2cap_check_fcs(chan, skb))
3927                         goto drop;
3928
3929                 if (__is_sar_start(control))
3930                         len -= 2;
3931
3932                 if (chan->fcs == L2CAP_FCS_CRC16)
3933                         len -= 2;
3934
3935                 if (len > chan->mps || len < 0 || __is_sframe(control))
3936                         goto drop;
3937
3938                 tx_seq = __get_txseq(control);
3939
3940                 if (chan->expected_tx_seq == tx_seq)
3941                         chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64;
3942                 else
3943                         chan->expected_tx_seq = (tx_seq + 1) % 64;
3944
3945                 l2cap_streaming_reassembly_sdu(chan, skb, control);
3946
3947                 goto done;
3948
3949         default:
3950                 BT_DBG("chan %p: bad mode 0x%2.2x", chan, chan->mode);
3951                 break;
3952         }
3953
3954 drop:
3955         kfree_skb(skb);
3956
3957 done:
3958         if (sk)
3959                 bh_unlock_sock(sk);
3960
3961         return 0;
3962 }
3963
3964 static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb)
3965 {
3966         struct sock *sk = NULL;
3967         struct l2cap_chan *chan;
3968
3969         chan = l2cap_global_chan_by_psm(0, psm, conn->src);
3970         if (!chan)
3971                 goto drop;
3972
3973         sk = chan->sk;
3974
3975         bh_lock_sock(sk);
3976
3977         BT_DBG("sk %p, len %d", sk, skb->len);
3978
3979         if (chan->state != BT_BOUND && chan->state != BT_CONNECTED)
3980                 goto drop;
3981
3982         if (chan->imtu < skb->len)
3983                 goto drop;
3984
3985         if (!chan->ops->recv(chan->data, skb))
3986                 goto done;
3987
3988 drop:
3989         kfree_skb(skb);
3990
3991 done:
3992         if (sk)
3993                 bh_unlock_sock(sk);
3994         return 0;
3995 }
3996
3997 static inline int l2cap_att_channel(struct l2cap_conn *conn, __le16 cid, struct sk_buff *skb)
3998 {
3999         struct sock *sk = NULL;
4000         struct l2cap_chan *chan;
4001
4002         chan = l2cap_global_chan_by_scid(0, cid, conn->src);
4003         if (!chan)
4004                 goto drop;
4005
4006         sk = chan->sk;
4007
4008         bh_lock_sock(sk);
4009
4010         BT_DBG("sk %p, len %d", sk, skb->len);
4011
4012         if (chan->state != BT_BOUND && chan->state != BT_CONNECTED)
4013                 goto drop;
4014
4015         if (chan->imtu < skb->len)
4016                 goto drop;
4017
4018         if (!chan->ops->recv(chan->data, skb))
4019                 goto done;
4020
4021 drop:
4022         kfree_skb(skb);
4023
4024 done:
4025         if (sk)
4026                 bh_unlock_sock(sk);
4027         return 0;
4028 }
4029
4030 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
4031 {
4032         struct l2cap_hdr *lh = (void *) skb->data;
4033         u16 cid, len;
4034         __le16 psm;
4035
4036         skb_pull(skb, L2CAP_HDR_SIZE);
4037         cid = __le16_to_cpu(lh->cid);
4038         len = __le16_to_cpu(lh->len);
4039
4040         if (len != skb->len) {
4041                 kfree_skb(skb);
4042                 return;
4043         }
4044
4045         BT_DBG("len %d, cid 0x%4.4x", len, cid);
4046
4047         switch (cid) {
4048         case L2CAP_CID_LE_SIGNALING:
4049         case L2CAP_CID_SIGNALING:
4050                 l2cap_sig_channel(conn, skb);
4051                 break;
4052
4053         case L2CAP_CID_CONN_LESS:
4054                 psm = get_unaligned_le16(skb->data);
4055                 skb_pull(skb, 2);
4056                 l2cap_conless_channel(conn, psm, skb);
4057                 break;
4058
4059         case L2CAP_CID_LE_DATA:
4060                 l2cap_att_channel(conn, cid, skb);
4061                 break;
4062
4063         case L2CAP_CID_SMP:
4064                 if (smp_sig_channel(conn, skb))
4065                         l2cap_conn_del(conn->hcon, EACCES);
4066                 break;
4067
4068         default:
4069                 l2cap_data_channel(conn, cid, skb);
4070                 break;
4071         }
4072 }
4073
4074 /* ---- L2CAP interface with lower layer (HCI) ---- */
4075
4076 static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
4077 {
4078         int exact = 0, lm1 = 0, lm2 = 0;
4079         struct l2cap_chan *c;
4080
4081         if (type != ACL_LINK)
4082                 return -EINVAL;
4083
4084         BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
4085
4086         /* Find listening sockets and check their link_mode */
4087         read_lock(&chan_list_lock);
4088         list_for_each_entry(c, &chan_list, global_l) {
4089                 struct sock *sk = c->sk;
4090
4091                 if (c->state != BT_LISTEN)
4092                         continue;
4093
4094                 if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
4095                         lm1 |= HCI_LM_ACCEPT;
4096                         if (c->role_switch)
4097                                 lm1 |= HCI_LM_MASTER;
4098                         exact++;
4099                 } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) {
4100                         lm2 |= HCI_LM_ACCEPT;
4101                         if (c->role_switch)
4102                                 lm2 |= HCI_LM_MASTER;
4103                 }
4104         }
4105         read_unlock(&chan_list_lock);
4106
4107         return exact ? lm1 : lm2;
4108 }
4109
4110 static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
4111 {
4112         struct l2cap_conn *conn;
4113
4114         BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
4115
4116         if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK))
4117                 return -EINVAL;
4118
4119         if (!status) {
4120                 conn = l2cap_conn_add(hcon, status);
4121                 if (conn)
4122                         l2cap_conn_ready(conn);
4123         } else
4124                 l2cap_conn_del(hcon, bt_to_errno(status));
4125
4126         return 0;
4127 }
4128
4129 static int l2cap_disconn_ind(struct hci_conn *hcon)
4130 {
4131         struct l2cap_conn *conn = hcon->l2cap_data;
4132
4133         BT_DBG("hcon %p", hcon);
4134
4135         if ((hcon->type != ACL_LINK && hcon->type != LE_LINK) || !conn)
4136                 return 0x13;
4137
4138         return conn->disc_reason;
4139 }
4140
4141 static int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
4142 {
4143         BT_DBG("hcon %p reason %d", hcon, reason);
4144
4145         if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK))
4146                 return -EINVAL;
4147
4148         l2cap_conn_del(hcon, bt_to_errno(reason));
4149
4150         return 0;
4151 }
4152
4153 static inline void l2cap_check_encryption(struct l2cap_chan *chan, u8 encrypt)
4154 {
4155         if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED)
4156                 return;
4157
4158         if (encrypt == 0x00) {
4159                 if (chan->sec_level == BT_SECURITY_MEDIUM) {
4160                         __clear_chan_timer(chan);
4161                         __set_chan_timer(chan, HZ * 5);
4162                 } else if (chan->sec_level == BT_SECURITY_HIGH)
4163                         l2cap_chan_close(chan, ECONNREFUSED);
4164         } else {
4165                 if (chan->sec_level == BT_SECURITY_MEDIUM)
4166                         __clear_chan_timer(chan);
4167         }
4168 }
4169
4170 static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
4171 {
4172         struct l2cap_conn *conn = hcon->l2cap_data;
4173         struct l2cap_chan *chan;
4174
4175         if (!conn)
4176                 return 0;
4177
4178         BT_DBG("conn %p", conn);
4179
4180         read_lock(&conn->chan_lock);
4181
4182         list_for_each_entry(chan, &conn->chan_l, list) {
4183                 struct sock *sk = chan->sk;
4184
4185                 bh_lock_sock(sk);
4186
4187                 BT_DBG("chan->scid %d", chan->scid);
4188
4189                 if (chan->scid == L2CAP_CID_LE_DATA) {
4190                         if (!status && encrypt) {
4191                                 chan->sec_level = hcon->sec_level;
4192                                 del_timer(&conn->security_timer);
4193                                 l2cap_chan_ready(sk);
4194                         }
4195
4196                         bh_unlock_sock(sk);
4197                         continue;
4198                 }
4199
4200                 if (test_bit(CONF_CONNECT_PEND, &chan->conf_state)) {
4201                         bh_unlock_sock(sk);
4202                         continue;
4203                 }
4204
4205                 if (!status && (chan->state == BT_CONNECTED ||
4206                                                 chan->state == BT_CONFIG)) {
4207                         l2cap_check_encryption(chan, encrypt);
4208                         bh_unlock_sock(sk);
4209                         continue;
4210                 }
4211
4212                 if (chan->state == BT_CONNECT) {
4213                         if (!status) {
4214                                 struct l2cap_conn_req req;
4215                                 req.scid = cpu_to_le16(chan->scid);
4216                                 req.psm  = chan->psm;
4217
4218                                 chan->ident = l2cap_get_ident(conn);
4219                                 set_bit(CONF_CONNECT_PEND, &chan->conf_state);
4220
4221                                 l2cap_send_cmd(conn, chan->ident,
4222                                         L2CAP_CONN_REQ, sizeof(req), &req);
4223                         } else {
4224                                 __clear_chan_timer(chan);
4225                                 __set_chan_timer(chan, HZ / 10);
4226                         }
4227                 } else if (chan->state == BT_CONNECT2) {
4228                         struct l2cap_conn_rsp rsp;
4229                         __u16 res, stat;
4230
4231                         if (!status) {
4232                                 if (bt_sk(sk)->defer_setup) {
4233                                         struct sock *parent = bt_sk(sk)->parent;
4234                                         res = L2CAP_CR_PEND;
4235                                         stat = L2CAP_CS_AUTHOR_PEND;
4236                                         parent->sk_data_ready(parent, 0);
4237                                 } else {
4238                                         l2cap_state_change(chan, BT_CONFIG);
4239                                         res = L2CAP_CR_SUCCESS;
4240                                         stat = L2CAP_CS_NO_INFO;
4241                                 }
4242                         } else {
4243                                 l2cap_state_change(chan, BT_DISCONN);
4244                                 __set_chan_timer(chan, HZ / 10);
4245                                 res = L2CAP_CR_SEC_BLOCK;
4246                                 stat = L2CAP_CS_NO_INFO;
4247                         }
4248
4249                         rsp.scid   = cpu_to_le16(chan->dcid);
4250                         rsp.dcid   = cpu_to_le16(chan->scid);
4251                         rsp.result = cpu_to_le16(res);
4252                         rsp.status = cpu_to_le16(stat);
4253                         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
4254                                                         sizeof(rsp), &rsp);
4255                 }
4256
4257                 bh_unlock_sock(sk);
4258         }
4259
4260         read_unlock(&conn->chan_lock);
4261
4262         return 0;
4263 }
4264
4265 static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
4266 {
4267         struct l2cap_conn *conn = hcon->l2cap_data;
4268
4269         if (!conn)
4270                 conn = l2cap_conn_add(hcon, 0);
4271
4272         if (!conn)
4273                 goto drop;
4274
4275         BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
4276
4277         if (!(flags & ACL_CONT)) {
4278                 struct l2cap_hdr *hdr;
4279                 struct l2cap_chan *chan;
4280                 u16 cid;
4281                 int len;
4282
4283                 if (conn->rx_len) {
4284                         BT_ERR("Unexpected start frame (len %d)", skb->len);
4285                         kfree_skb(conn->rx_skb);
4286                         conn->rx_skb = NULL;
4287                         conn->rx_len = 0;
4288                         l2cap_conn_unreliable(conn, ECOMM);
4289                 }
4290
4291                 /* Start fragment always begin with Basic L2CAP header */
4292                 if (skb->len < L2CAP_HDR_SIZE) {
4293                         BT_ERR("Frame is too short (len %d)", skb->len);
4294                         l2cap_conn_unreliable(conn, ECOMM);
4295                         goto drop;
4296                 }
4297
4298                 hdr = (struct l2cap_hdr *) skb->data;
4299                 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
4300                 cid = __le16_to_cpu(hdr->cid);
4301
4302                 if (len == skb->len) {
4303                         /* Complete frame received */
4304                         l2cap_recv_frame(conn, skb);
4305                         return 0;
4306                 }
4307
4308                 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
4309
4310                 if (skb->len > len) {
4311                         BT_ERR("Frame is too long (len %d, expected len %d)",
4312                                 skb->len, len);
4313                         l2cap_conn_unreliable(conn, ECOMM);
4314                         goto drop;
4315                 }
4316
4317                 chan = l2cap_get_chan_by_scid(conn, cid);
4318
4319                 if (chan && chan->sk) {
4320                         struct sock *sk = chan->sk;
4321
4322                         if (chan->imtu < len - L2CAP_HDR_SIZE) {
4323                                 BT_ERR("Frame exceeding recv MTU (len %d, "
4324                                                         "MTU %d)", len,
4325                                                         chan->imtu);
4326                                 bh_unlock_sock(sk);
4327                                 l2cap_conn_unreliable(conn, ECOMM);
4328                                 goto drop;
4329                         }
4330                         bh_unlock_sock(sk);
4331                 }
4332
4333                 /* Allocate skb for the complete frame (with header) */
4334                 conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC);
4335                 if (!conn->rx_skb)
4336                         goto drop;
4337
4338                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
4339                                                                 skb->len);
4340                 conn->rx_len = len - skb->len;
4341         } else {
4342                 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
4343
4344                 if (!conn->rx_len) {
4345                         BT_ERR("Unexpected continuation frame (len %d)", skb->len);
4346                         l2cap_conn_unreliable(conn, ECOMM);
4347                         goto drop;
4348                 }
4349
4350                 if (skb->len > conn->rx_len) {
4351                         BT_ERR("Fragment is too long (len %d, expected %d)",
4352                                         skb->len, conn->rx_len);
4353                         kfree_skb(conn->rx_skb);
4354                         conn->rx_skb = NULL;
4355                         conn->rx_len = 0;
4356                         l2cap_conn_unreliable(conn, ECOMM);
4357                         goto drop;
4358                 }
4359
4360                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
4361                                                                 skb->len);
4362                 conn->rx_len -= skb->len;
4363
4364                 if (!conn->rx_len) {
4365                         /* Complete frame received */
4366                         l2cap_recv_frame(conn, conn->rx_skb);
4367                         conn->rx_skb = NULL;
4368                 }
4369         }
4370
4371 drop:
4372         kfree_skb(skb);
4373         return 0;
4374 }
4375
4376 static int l2cap_debugfs_show(struct seq_file *f, void *p)
4377 {
4378         struct l2cap_chan *c;
4379
4380         read_lock_bh(&chan_list_lock);
4381
4382         list_for_each_entry(c, &chan_list, global_l) {
4383                 struct sock *sk = c->sk;
4384
4385                 seq_printf(f, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n",
4386                                         batostr(&bt_sk(sk)->src),
4387                                         batostr(&bt_sk(sk)->dst),
4388                                         c->state, __le16_to_cpu(c->psm),
4389                                         c->scid, c->dcid, c->imtu, c->omtu,
4390                                         c->sec_level, c->mode);
4391 }
4392
4393         read_unlock_bh(&chan_list_lock);
4394
4395         return 0;
4396 }
4397
4398 static int l2cap_debugfs_open(struct inode *inode, struct file *file)
4399 {
4400         return single_open(file, l2cap_debugfs_show, inode->i_private);
4401 }
4402
4403 static const struct file_operations l2cap_debugfs_fops = {
4404         .open           = l2cap_debugfs_open,
4405         .read           = seq_read,
4406         .llseek         = seq_lseek,
4407         .release        = single_release,
4408 };
4409
4410 static struct dentry *l2cap_debugfs;
4411
4412 static struct hci_proto l2cap_hci_proto = {
4413         .name           = "L2CAP",
4414         .id             = HCI_PROTO_L2CAP,
4415         .connect_ind    = l2cap_connect_ind,
4416         .connect_cfm    = l2cap_connect_cfm,
4417         .disconn_ind    = l2cap_disconn_ind,
4418         .disconn_cfm    = l2cap_disconn_cfm,
4419         .security_cfm   = l2cap_security_cfm,
4420         .recv_acldata   = l2cap_recv_acldata
4421 };
4422
4423 int __init l2cap_init(void)
4424 {
4425         int err;
4426
4427         err = l2cap_init_sockets();
4428         if (err < 0)
4429                 return err;
4430
4431         _busy_wq = create_singlethread_workqueue("l2cap");
4432         if (!_busy_wq) {
4433                 err = -ENOMEM;
4434                 goto error;
4435         }
4436
4437         err = hci_register_proto(&l2cap_hci_proto);
4438         if (err < 0) {
4439                 BT_ERR("L2CAP protocol registration failed");
4440                 bt_sock_unregister(BTPROTO_L2CAP);
4441                 goto error;
4442         }
4443
4444         if (bt_debugfs) {
4445                 l2cap_debugfs = debugfs_create_file("l2cap", 0444,
4446                                         bt_debugfs, NULL, &l2cap_debugfs_fops);
4447                 if (!l2cap_debugfs)
4448                         BT_ERR("Failed to create L2CAP debug file");
4449         }
4450
4451         return 0;
4452
4453 error:
4454         destroy_workqueue(_busy_wq);
4455         l2cap_cleanup_sockets();
4456         return err;
4457 }
4458
4459 void l2cap_exit(void)
4460 {
4461         debugfs_remove(l2cap_debugfs);
4462
4463         flush_workqueue(_busy_wq);
4464         destroy_workqueue(_busy_wq);
4465
4466         if (hci_unregister_proto(&l2cap_hci_proto) < 0)
4467                 BT_ERR("L2CAP protocol unregistration failed");
4468
4469         l2cap_cleanup_sockets();
4470 }
4471
4472 module_param(disable_ertm, bool, 0644);
4473 MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode");