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