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