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