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