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