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