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