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