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