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