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