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