Bluetooth: Fix issues where sk_sleep() helper is needed now
[pandora-kernel.git] / net / bluetooth / l2cap.c
1 /*
2    BlueZ - Bluetooth protocol stack for Linux
3    Copyright (C) 2000-2001 Qualcomm Incorporated
4
5    Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.com>
6
7    This program is free software; you can redistribute it and/or modify
8    it under the terms of the GNU General Public License version 2 as
9    published by the Free Software Foundation;
10
11    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
12    OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
13    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS.
14    IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY
15    CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES
16    WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
17    ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
18    OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
19
20    ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS,
21    COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS
22    SOFTWARE IS DISCLAIMED.
23 */
24
25 /* Bluetooth L2CAP core and sockets. */
26
27 #include <linux/module.h>
28
29 #include <linux/types.h>
30 #include <linux/capability.h>
31 #include <linux/errno.h>
32 #include <linux/kernel.h>
33 #include <linux/sched.h>
34 #include <linux/slab.h>
35 #include <linux/poll.h>
36 #include <linux/fcntl.h>
37 #include <linux/init.h>
38 #include <linux/interrupt.h>
39 #include <linux/socket.h>
40 #include <linux/skbuff.h>
41 #include <linux/list.h>
42 #include <linux/device.h>
43 #include <linux/debugfs.h>
44 #include <linux/seq_file.h>
45 #include <linux/uaccess.h>
46 #include <linux/crc16.h>
47 #include <net/sock.h>
48
49 #include <asm/system.h>
50 #include <asm/unaligned.h>
51
52 #include <net/bluetooth/bluetooth.h>
53 #include <net/bluetooth/hci_core.h>
54 #include <net/bluetooth/l2cap.h>
55
56 #define VERSION "2.14"
57
58 #ifdef CONFIG_BT_L2CAP_EXT_FEATURES
59 static int enable_ertm = 1;
60 #else
61 static int enable_ertm = 0;
62 #endif
63 static int max_transmit = L2CAP_DEFAULT_MAX_TX;
64 static int tx_window = L2CAP_DEFAULT_TX_WINDOW;
65
66 static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN;
67 static u8 l2cap_fixed_chan[8] = { 0x02, };
68
69 static const struct proto_ops l2cap_sock_ops;
70
71 static struct workqueue_struct *_busy_wq;
72
73 static struct bt_sock_list l2cap_sk_list = {
74         .lock = __RW_LOCK_UNLOCKED(l2cap_sk_list.lock)
75 };
76
77 static void l2cap_busy_work(struct work_struct *work);
78
79 static void __l2cap_sock_close(struct sock *sk, int reason);
80 static void l2cap_sock_close(struct sock *sk);
81 static void l2cap_sock_kill(struct sock *sk);
82
83 static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
84                                 u8 code, u8 ident, u16 dlen, void *data);
85
86 /* ---- L2CAP timers ---- */
87 static void l2cap_sock_timeout(unsigned long arg)
88 {
89         struct sock *sk = (struct sock *) arg;
90         int reason;
91
92         BT_DBG("sock %p state %d", sk, sk->sk_state);
93
94         bh_lock_sock(sk);
95
96         if (sk->sk_state == BT_CONNECTED || sk->sk_state == BT_CONFIG)
97                 reason = ECONNREFUSED;
98         else if (sk->sk_state == BT_CONNECT &&
99                                 l2cap_pi(sk)->sec_level != BT_SECURITY_SDP)
100                 reason = ECONNREFUSED;
101         else
102                 reason = ETIMEDOUT;
103
104         __l2cap_sock_close(sk, reason);
105
106         bh_unlock_sock(sk);
107
108         l2cap_sock_kill(sk);
109         sock_put(sk);
110 }
111
112 static void l2cap_sock_set_timer(struct sock *sk, long timeout)
113 {
114         BT_DBG("sk %p state %d timeout %ld", sk, sk->sk_state, timeout);
115         sk_reset_timer(sk, &sk->sk_timer, jiffies + timeout);
116 }
117
118 static void l2cap_sock_clear_timer(struct sock *sk)
119 {
120         BT_DBG("sock %p state %d", sk, sk->sk_state);
121         sk_stop_timer(sk, &sk->sk_timer);
122 }
123
124 /* ---- L2CAP channels ---- */
125 static struct sock *__l2cap_get_chan_by_dcid(struct l2cap_chan_list *l, u16 cid)
126 {
127         struct sock *s;
128         for (s = l->head; s; s = l2cap_pi(s)->next_c) {
129                 if (l2cap_pi(s)->dcid == cid)
130                         break;
131         }
132         return s;
133 }
134
135 static struct sock *__l2cap_get_chan_by_scid(struct l2cap_chan_list *l, u16 cid)
136 {
137         struct sock *s;
138         for (s = l->head; s; s = l2cap_pi(s)->next_c) {
139                 if (l2cap_pi(s)->scid == cid)
140                         break;
141         }
142         return s;
143 }
144
145 /* Find channel with given SCID.
146  * Returns locked socket */
147 static inline struct sock *l2cap_get_chan_by_scid(struct l2cap_chan_list *l, u16 cid)
148 {
149         struct sock *s;
150         read_lock(&l->lock);
151         s = __l2cap_get_chan_by_scid(l, cid);
152         if (s)
153                 bh_lock_sock(s);
154         read_unlock(&l->lock);
155         return s;
156 }
157
158 static struct sock *__l2cap_get_chan_by_ident(struct l2cap_chan_list *l, u8 ident)
159 {
160         struct sock *s;
161         for (s = l->head; s; s = l2cap_pi(s)->next_c) {
162                 if (l2cap_pi(s)->ident == ident)
163                         break;
164         }
165         return s;
166 }
167
168 static inline struct sock *l2cap_get_chan_by_ident(struct l2cap_chan_list *l, u8 ident)
169 {
170         struct sock *s;
171         read_lock(&l->lock);
172         s = __l2cap_get_chan_by_ident(l, ident);
173         if (s)
174                 bh_lock_sock(s);
175         read_unlock(&l->lock);
176         return s;
177 }
178
179 static u16 l2cap_alloc_cid(struct l2cap_chan_list *l)
180 {
181         u16 cid = L2CAP_CID_DYN_START;
182
183         for (; cid < L2CAP_CID_DYN_END; cid++) {
184                 if (!__l2cap_get_chan_by_scid(l, cid))
185                         return cid;
186         }
187
188         return 0;
189 }
190
191 static inline void __l2cap_chan_link(struct l2cap_chan_list *l, struct sock *sk)
192 {
193         sock_hold(sk);
194
195         if (l->head)
196                 l2cap_pi(l->head)->prev_c = sk;
197
198         l2cap_pi(sk)->next_c = l->head;
199         l2cap_pi(sk)->prev_c = NULL;
200         l->head = sk;
201 }
202
203 static inline void l2cap_chan_unlink(struct l2cap_chan_list *l, struct sock *sk)
204 {
205         struct sock *next = l2cap_pi(sk)->next_c, *prev = l2cap_pi(sk)->prev_c;
206
207         write_lock_bh(&l->lock);
208         if (sk == l->head)
209                 l->head = next;
210
211         if (next)
212                 l2cap_pi(next)->prev_c = prev;
213         if (prev)
214                 l2cap_pi(prev)->next_c = next;
215         write_unlock_bh(&l->lock);
216
217         __sock_put(sk);
218 }
219
220 static void __l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct sock *parent)
221 {
222         struct l2cap_chan_list *l = &conn->chan_list;
223
224         BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn,
225                         l2cap_pi(sk)->psm, l2cap_pi(sk)->dcid);
226
227         conn->disc_reason = 0x13;
228
229         l2cap_pi(sk)->conn = conn;
230
231         if (sk->sk_type == SOCK_SEQPACKET || sk->sk_type == SOCK_STREAM) {
232                 /* Alloc CID for connection-oriented socket */
233                 l2cap_pi(sk)->scid = l2cap_alloc_cid(l);
234         } else if (sk->sk_type == SOCK_DGRAM) {
235                 /* Connectionless socket */
236                 l2cap_pi(sk)->scid = L2CAP_CID_CONN_LESS;
237                 l2cap_pi(sk)->dcid = L2CAP_CID_CONN_LESS;
238                 l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU;
239         } else {
240                 /* Raw socket can send/recv signalling messages only */
241                 l2cap_pi(sk)->scid = L2CAP_CID_SIGNALING;
242                 l2cap_pi(sk)->dcid = L2CAP_CID_SIGNALING;
243                 l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU;
244         }
245
246         __l2cap_chan_link(l, sk);
247
248         if (parent)
249                 bt_accept_enqueue(parent, sk);
250 }
251
252 /* Delete channel.
253  * Must be called on the locked socket. */
254 static void l2cap_chan_del(struct sock *sk, int err)
255 {
256         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
257         struct sock *parent = bt_sk(sk)->parent;
258
259         l2cap_sock_clear_timer(sk);
260
261         BT_DBG("sk %p, conn %p, err %d", sk, conn, err);
262
263         if (conn) {
264                 /* Unlink from channel list */
265                 l2cap_chan_unlink(&conn->chan_list, sk);
266                 l2cap_pi(sk)->conn = NULL;
267                 hci_conn_put(conn->hcon);
268         }
269
270         sk->sk_state = BT_CLOSED;
271         sock_set_flag(sk, SOCK_ZAPPED);
272
273         if (err)
274                 sk->sk_err = err;
275
276         if (parent) {
277                 bt_accept_unlink(sk);
278                 parent->sk_data_ready(parent, 0);
279         } else
280                 sk->sk_state_change(sk);
281 }
282
283 /* Service level security */
284 static inline int l2cap_check_security(struct sock *sk)
285 {
286         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
287         __u8 auth_type;
288
289         if (l2cap_pi(sk)->psm == cpu_to_le16(0x0001)) {
290                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_HIGH)
291                         auth_type = HCI_AT_NO_BONDING_MITM;
292                 else
293                         auth_type = HCI_AT_NO_BONDING;
294
295                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_LOW)
296                         l2cap_pi(sk)->sec_level = BT_SECURITY_SDP;
297         } else {
298                 switch (l2cap_pi(sk)->sec_level) {
299                 case BT_SECURITY_HIGH:
300                         auth_type = HCI_AT_GENERAL_BONDING_MITM;
301                         break;
302                 case BT_SECURITY_MEDIUM:
303                         auth_type = HCI_AT_GENERAL_BONDING;
304                         break;
305                 default:
306                         auth_type = HCI_AT_NO_BONDING;
307                         break;
308                 }
309         }
310
311         return hci_conn_security(conn->hcon, l2cap_pi(sk)->sec_level,
312                                                                 auth_type);
313 }
314
315 static inline u8 l2cap_get_ident(struct l2cap_conn *conn)
316 {
317         u8 id;
318
319         /* Get next available identificator.
320          *    1 - 128 are used by kernel.
321          *  129 - 199 are reserved.
322          *  200 - 254 are used by utilities like l2ping, etc.
323          */
324
325         spin_lock_bh(&conn->lock);
326
327         if (++conn->tx_ident > 128)
328                 conn->tx_ident = 1;
329
330         id = conn->tx_ident;
331
332         spin_unlock_bh(&conn->lock);
333
334         return id;
335 }
336
337 static inline void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, void *data)
338 {
339         struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data);
340
341         BT_DBG("code 0x%2.2x", code);
342
343         if (!skb)
344                 return;
345
346         hci_send_acl(conn->hcon, skb, 0);
347 }
348
349 static inline void l2cap_send_sframe(struct l2cap_pinfo *pi, u16 control)
350 {
351         struct sk_buff *skb;
352         struct l2cap_hdr *lh;
353         struct l2cap_conn *conn = pi->conn;
354         int count, hlen = L2CAP_HDR_SIZE + 2;
355
356         if (pi->fcs == L2CAP_FCS_CRC16)
357                 hlen += 2;
358
359         BT_DBG("pi %p, control 0x%2.2x", pi, control);
360
361         count = min_t(unsigned int, conn->mtu, hlen);
362         control |= L2CAP_CTRL_FRAME_TYPE;
363
364         if (pi->conn_state & L2CAP_CONN_SEND_FBIT) {
365                 control |= L2CAP_CTRL_FINAL;
366                 pi->conn_state &= ~L2CAP_CONN_SEND_FBIT;
367         }
368
369         if (pi->conn_state & L2CAP_CONN_SEND_PBIT) {
370                 control |= L2CAP_CTRL_POLL;
371                 pi->conn_state &= ~L2CAP_CONN_SEND_PBIT;
372         }
373
374         skb = bt_skb_alloc(count, GFP_ATOMIC);
375         if (!skb)
376                 return;
377
378         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
379         lh->len = cpu_to_le16(hlen - L2CAP_HDR_SIZE);
380         lh->cid = cpu_to_le16(pi->dcid);
381         put_unaligned_le16(control, skb_put(skb, 2));
382
383         if (pi->fcs == L2CAP_FCS_CRC16) {
384                 u16 fcs = crc16(0, (u8 *)lh, count - 2);
385                 put_unaligned_le16(fcs, skb_put(skb, 2));
386         }
387
388         hci_send_acl(pi->conn->hcon, skb, 0);
389 }
390
391 static inline void l2cap_send_rr_or_rnr(struct l2cap_pinfo *pi, u16 control)
392 {
393         if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
394                 control |= L2CAP_SUPER_RCV_NOT_READY;
395                 pi->conn_state |= L2CAP_CONN_RNR_SENT;
396         } else
397                 control |= L2CAP_SUPER_RCV_READY;
398
399         control |= pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
400
401         l2cap_send_sframe(pi, control);
402 }
403
404 static void l2cap_do_start(struct sock *sk)
405 {
406         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
407
408         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) {
409                 if (!(conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE))
410                         return;
411
412                 if (l2cap_check_security(sk)) {
413                         struct l2cap_conn_req req;
414                         req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
415                         req.psm  = l2cap_pi(sk)->psm;
416
417                         l2cap_pi(sk)->ident = l2cap_get_ident(conn);
418
419                         l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
420                                         L2CAP_CONN_REQ, sizeof(req), &req);
421                 }
422         } else {
423                 struct l2cap_info_req req;
424                 req.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
425
426                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
427                 conn->info_ident = l2cap_get_ident(conn);
428
429                 mod_timer(&conn->info_timer, jiffies +
430                                         msecs_to_jiffies(L2CAP_INFO_TIMEOUT));
431
432                 l2cap_send_cmd(conn, conn->info_ident,
433                                         L2CAP_INFO_REQ, sizeof(req), &req);
434         }
435 }
436
437 static void l2cap_send_disconn_req(struct l2cap_conn *conn, struct sock *sk)
438 {
439         struct l2cap_disconn_req req;
440
441         req.dcid = cpu_to_le16(l2cap_pi(sk)->dcid);
442         req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
443         l2cap_send_cmd(conn, l2cap_get_ident(conn),
444                         L2CAP_DISCONN_REQ, sizeof(req), &req);
445 }
446
447 /* ---- L2CAP connections ---- */
448 static void l2cap_conn_start(struct l2cap_conn *conn)
449 {
450         struct l2cap_chan_list *l = &conn->chan_list;
451         struct sock *sk;
452
453         BT_DBG("conn %p", conn);
454
455         read_lock(&l->lock);
456
457         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
458                 bh_lock_sock(sk);
459
460                 if (sk->sk_type != SOCK_SEQPACKET &&
461                                 sk->sk_type != SOCK_STREAM) {
462                         bh_unlock_sock(sk);
463                         continue;
464                 }
465
466                 if (sk->sk_state == BT_CONNECT) {
467                         if (l2cap_check_security(sk)) {
468                                 struct l2cap_conn_req req;
469                                 req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
470                                 req.psm  = l2cap_pi(sk)->psm;
471
472                                 l2cap_pi(sk)->ident = l2cap_get_ident(conn);
473
474                                 l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
475                                         L2CAP_CONN_REQ, sizeof(req), &req);
476                         }
477                 } else if (sk->sk_state == BT_CONNECT2) {
478                         struct l2cap_conn_rsp rsp;
479                         rsp.scid = cpu_to_le16(l2cap_pi(sk)->dcid);
480                         rsp.dcid = cpu_to_le16(l2cap_pi(sk)->scid);
481
482                         if (l2cap_check_security(sk)) {
483                                 if (bt_sk(sk)->defer_setup) {
484                                         struct sock *parent = bt_sk(sk)->parent;
485                                         rsp.result = cpu_to_le16(L2CAP_CR_PEND);
486                                         rsp.status = cpu_to_le16(L2CAP_CS_AUTHOR_PEND);
487                                         parent->sk_data_ready(parent, 0);
488
489                                 } else {
490                                         sk->sk_state = BT_CONFIG;
491                                         rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
492                                         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
493                                 }
494                         } else {
495                                 rsp.result = cpu_to_le16(L2CAP_CR_PEND);
496                                 rsp.status = cpu_to_le16(L2CAP_CS_AUTHEN_PEND);
497                         }
498
499                         l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
500                                         L2CAP_CONN_RSP, sizeof(rsp), &rsp);
501                 }
502
503                 bh_unlock_sock(sk);
504         }
505
506         read_unlock(&l->lock);
507 }
508
509 static void l2cap_conn_ready(struct l2cap_conn *conn)
510 {
511         struct l2cap_chan_list *l = &conn->chan_list;
512         struct sock *sk;
513
514         BT_DBG("conn %p", conn);
515
516         read_lock(&l->lock);
517
518         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
519                 bh_lock_sock(sk);
520
521                 if (sk->sk_type != SOCK_SEQPACKET &&
522                                 sk->sk_type != SOCK_STREAM) {
523                         l2cap_sock_clear_timer(sk);
524                         sk->sk_state = BT_CONNECTED;
525                         sk->sk_state_change(sk);
526                 } else if (sk->sk_state == BT_CONNECT)
527                         l2cap_do_start(sk);
528
529                 bh_unlock_sock(sk);
530         }
531
532         read_unlock(&l->lock);
533 }
534
535 /* Notify sockets that we cannot guaranty reliability anymore */
536 static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err)
537 {
538         struct l2cap_chan_list *l = &conn->chan_list;
539         struct sock *sk;
540
541         BT_DBG("conn %p", conn);
542
543         read_lock(&l->lock);
544
545         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
546                 if (l2cap_pi(sk)->force_reliable)
547                         sk->sk_err = err;
548         }
549
550         read_unlock(&l->lock);
551 }
552
553 static void l2cap_info_timeout(unsigned long arg)
554 {
555         struct l2cap_conn *conn = (void *) arg;
556
557         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
558         conn->info_ident = 0;
559
560         l2cap_conn_start(conn);
561 }
562
563 static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status)
564 {
565         struct l2cap_conn *conn = hcon->l2cap_data;
566
567         if (conn || status)
568                 return conn;
569
570         conn = kzalloc(sizeof(struct l2cap_conn), GFP_ATOMIC);
571         if (!conn)
572                 return NULL;
573
574         hcon->l2cap_data = conn;
575         conn->hcon = hcon;
576
577         BT_DBG("hcon %p conn %p", hcon, conn);
578
579         conn->mtu = hcon->hdev->acl_mtu;
580         conn->src = &hcon->hdev->bdaddr;
581         conn->dst = &hcon->dst;
582
583         conn->feat_mask = 0;
584
585         spin_lock_init(&conn->lock);
586         rwlock_init(&conn->chan_list.lock);
587
588         setup_timer(&conn->info_timer, l2cap_info_timeout,
589                                                 (unsigned long) conn);
590
591         conn->disc_reason = 0x13;
592
593         return conn;
594 }
595
596 static void l2cap_conn_del(struct hci_conn *hcon, int err)
597 {
598         struct l2cap_conn *conn = hcon->l2cap_data;
599         struct sock *sk;
600
601         if (!conn)
602                 return;
603
604         BT_DBG("hcon %p conn %p, err %d", hcon, conn, err);
605
606         kfree_skb(conn->rx_skb);
607
608         /* Kill channels */
609         while ((sk = conn->chan_list.head)) {
610                 bh_lock_sock(sk);
611                 l2cap_chan_del(sk, err);
612                 bh_unlock_sock(sk);
613                 l2cap_sock_kill(sk);
614         }
615
616         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT)
617                 del_timer_sync(&conn->info_timer);
618
619         hcon->l2cap_data = NULL;
620         kfree(conn);
621 }
622
623 static inline void l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct sock *parent)
624 {
625         struct l2cap_chan_list *l = &conn->chan_list;
626         write_lock_bh(&l->lock);
627         __l2cap_chan_add(conn, sk, parent);
628         write_unlock_bh(&l->lock);
629 }
630
631 /* ---- Socket interface ---- */
632 static struct sock *__l2cap_get_sock_by_addr(__le16 psm, bdaddr_t *src)
633 {
634         struct sock *sk;
635         struct hlist_node *node;
636         sk_for_each(sk, node, &l2cap_sk_list.head)
637                 if (l2cap_pi(sk)->sport == psm && !bacmp(&bt_sk(sk)->src, src))
638                         goto found;
639         sk = NULL;
640 found:
641         return sk;
642 }
643
644 /* Find socket with psm and source bdaddr.
645  * Returns closest match.
646  */
647 static struct sock *__l2cap_get_sock_by_psm(int state, __le16 psm, bdaddr_t *src)
648 {
649         struct sock *sk = NULL, *sk1 = NULL;
650         struct hlist_node *node;
651
652         sk_for_each(sk, node, &l2cap_sk_list.head) {
653                 if (state && sk->sk_state != state)
654                         continue;
655
656                 if (l2cap_pi(sk)->psm == psm) {
657                         /* Exact match. */
658                         if (!bacmp(&bt_sk(sk)->src, src))
659                                 break;
660
661                         /* Closest match */
662                         if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY))
663                                 sk1 = sk;
664                 }
665         }
666         return node ? sk : sk1;
667 }
668
669 /* Find socket with given address (psm, src).
670  * Returns locked socket */
671 static inline struct sock *l2cap_get_sock_by_psm(int state, __le16 psm, bdaddr_t *src)
672 {
673         struct sock *s;
674         read_lock(&l2cap_sk_list.lock);
675         s = __l2cap_get_sock_by_psm(state, psm, src);
676         if (s)
677                 bh_lock_sock(s);
678         read_unlock(&l2cap_sk_list.lock);
679         return s;
680 }
681
682 static void l2cap_sock_destruct(struct sock *sk)
683 {
684         BT_DBG("sk %p", sk);
685
686         skb_queue_purge(&sk->sk_receive_queue);
687         skb_queue_purge(&sk->sk_write_queue);
688 }
689
690 static void l2cap_sock_cleanup_listen(struct sock *parent)
691 {
692         struct sock *sk;
693
694         BT_DBG("parent %p", parent);
695
696         /* Close not yet accepted channels */
697         while ((sk = bt_accept_dequeue(parent, NULL)))
698                 l2cap_sock_close(sk);
699
700         parent->sk_state = BT_CLOSED;
701         sock_set_flag(parent, SOCK_ZAPPED);
702 }
703
704 /* Kill socket (only if zapped and orphan)
705  * Must be called on unlocked socket.
706  */
707 static void l2cap_sock_kill(struct sock *sk)
708 {
709         if (!sock_flag(sk, SOCK_ZAPPED) || sk->sk_socket)
710                 return;
711
712         BT_DBG("sk %p state %d", sk, sk->sk_state);
713
714         /* Kill poor orphan */
715         bt_sock_unlink(&l2cap_sk_list, sk);
716         sock_set_flag(sk, SOCK_DEAD);
717         sock_put(sk);
718 }
719
720 static void __l2cap_sock_close(struct sock *sk, int reason)
721 {
722         BT_DBG("sk %p state %d socket %p", sk, sk->sk_state, sk->sk_socket);
723
724         switch (sk->sk_state) {
725         case BT_LISTEN:
726                 l2cap_sock_cleanup_listen(sk);
727                 break;
728
729         case BT_CONNECTED:
730         case BT_CONFIG:
731                 if (sk->sk_type == SOCK_SEQPACKET ||
732                                 sk->sk_type == SOCK_STREAM) {
733                         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
734
735                         sk->sk_state = BT_DISCONN;
736                         l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
737                         l2cap_send_disconn_req(conn, sk);
738                 } else
739                         l2cap_chan_del(sk, reason);
740                 break;
741
742         case BT_CONNECT2:
743                 if (sk->sk_type == SOCK_SEQPACKET ||
744                                 sk->sk_type == SOCK_STREAM) {
745                         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
746                         struct l2cap_conn_rsp rsp;
747                         __u16 result;
748
749                         if (bt_sk(sk)->defer_setup)
750                                 result = L2CAP_CR_SEC_BLOCK;
751                         else
752                                 result = L2CAP_CR_BAD_PSM;
753
754                         rsp.scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
755                         rsp.dcid   = cpu_to_le16(l2cap_pi(sk)->scid);
756                         rsp.result = cpu_to_le16(result);
757                         rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
758                         l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
759                                         L2CAP_CONN_RSP, sizeof(rsp), &rsp);
760                 } else
761                         l2cap_chan_del(sk, reason);
762                 break;
763
764         case BT_CONNECT:
765         case BT_DISCONN:
766                 l2cap_chan_del(sk, reason);
767                 break;
768
769         default:
770                 sock_set_flag(sk, SOCK_ZAPPED);
771                 break;
772         }
773 }
774
775 /* Must be called on unlocked socket. */
776 static void l2cap_sock_close(struct sock *sk)
777 {
778         l2cap_sock_clear_timer(sk);
779         lock_sock(sk);
780         __l2cap_sock_close(sk, ECONNRESET);
781         release_sock(sk);
782         l2cap_sock_kill(sk);
783 }
784
785 static void l2cap_sock_init(struct sock *sk, struct sock *parent)
786 {
787         struct l2cap_pinfo *pi = l2cap_pi(sk);
788
789         BT_DBG("sk %p", sk);
790
791         if (parent) {
792                 sk->sk_type = parent->sk_type;
793                 bt_sk(sk)->defer_setup = bt_sk(parent)->defer_setup;
794
795                 pi->imtu = l2cap_pi(parent)->imtu;
796                 pi->omtu = l2cap_pi(parent)->omtu;
797                 pi->mode = l2cap_pi(parent)->mode;
798                 pi->fcs  = l2cap_pi(parent)->fcs;
799                 pi->max_tx = l2cap_pi(parent)->max_tx;
800                 pi->tx_win = l2cap_pi(parent)->tx_win;
801                 pi->sec_level = l2cap_pi(parent)->sec_level;
802                 pi->role_switch = l2cap_pi(parent)->role_switch;
803                 pi->force_reliable = l2cap_pi(parent)->force_reliable;
804         } else {
805                 pi->imtu = L2CAP_DEFAULT_MTU;
806                 pi->omtu = 0;
807                 if (enable_ertm && sk->sk_type == SOCK_STREAM)
808                         pi->mode = L2CAP_MODE_ERTM;
809                 else
810                         pi->mode = L2CAP_MODE_BASIC;
811                 pi->max_tx = max_transmit;
812                 pi->fcs  = L2CAP_FCS_CRC16;
813                 pi->tx_win = tx_window;
814                 pi->sec_level = BT_SECURITY_LOW;
815                 pi->role_switch = 0;
816                 pi->force_reliable = 0;
817         }
818
819         /* Default config options */
820         pi->conf_len = 0;
821         pi->flush_to = L2CAP_DEFAULT_FLUSH_TO;
822         skb_queue_head_init(TX_QUEUE(sk));
823         skb_queue_head_init(SREJ_QUEUE(sk));
824         skb_queue_head_init(BUSY_QUEUE(sk));
825         INIT_LIST_HEAD(SREJ_LIST(sk));
826 }
827
828 static struct proto l2cap_proto = {
829         .name           = "L2CAP",
830         .owner          = THIS_MODULE,
831         .obj_size       = sizeof(struct l2cap_pinfo)
832 };
833
834 static struct sock *l2cap_sock_alloc(struct net *net, struct socket *sock, int proto, gfp_t prio)
835 {
836         struct sock *sk;
837
838         sk = sk_alloc(net, PF_BLUETOOTH, prio, &l2cap_proto);
839         if (!sk)
840                 return NULL;
841
842         sock_init_data(sock, sk);
843         INIT_LIST_HEAD(&bt_sk(sk)->accept_q);
844
845         sk->sk_destruct = l2cap_sock_destruct;
846         sk->sk_sndtimeo = msecs_to_jiffies(L2CAP_CONN_TIMEOUT);
847
848         sock_reset_flag(sk, SOCK_ZAPPED);
849
850         sk->sk_protocol = proto;
851         sk->sk_state = BT_OPEN;
852
853         setup_timer(&sk->sk_timer, l2cap_sock_timeout, (unsigned long) sk);
854
855         bt_sock_link(&l2cap_sk_list, sk);
856         return sk;
857 }
858
859 static int l2cap_sock_create(struct net *net, struct socket *sock, int protocol,
860                              int kern)
861 {
862         struct sock *sk;
863
864         BT_DBG("sock %p", sock);
865
866         sock->state = SS_UNCONNECTED;
867
868         if (sock->type != SOCK_SEQPACKET && sock->type != SOCK_STREAM &&
869                         sock->type != SOCK_DGRAM && sock->type != SOCK_RAW)
870                 return -ESOCKTNOSUPPORT;
871
872         if (sock->type == SOCK_RAW && !kern && !capable(CAP_NET_RAW))
873                 return -EPERM;
874
875         sock->ops = &l2cap_sock_ops;
876
877         sk = l2cap_sock_alloc(net, sock, protocol, GFP_ATOMIC);
878         if (!sk)
879                 return -ENOMEM;
880
881         l2cap_sock_init(sk, NULL);
882         return 0;
883 }
884
885 static int l2cap_sock_bind(struct socket *sock, struct sockaddr *addr, int alen)
886 {
887         struct sock *sk = sock->sk;
888         struct sockaddr_l2 la;
889         int len, err = 0;
890
891         BT_DBG("sk %p", sk);
892
893         if (!addr || addr->sa_family != AF_BLUETOOTH)
894                 return -EINVAL;
895
896         memset(&la, 0, sizeof(la));
897         len = min_t(unsigned int, sizeof(la), alen);
898         memcpy(&la, addr, len);
899
900         if (la.l2_cid)
901                 return -EINVAL;
902
903         lock_sock(sk);
904
905         if (sk->sk_state != BT_OPEN) {
906                 err = -EBADFD;
907                 goto done;
908         }
909
910         if (la.l2_psm && __le16_to_cpu(la.l2_psm) < 0x1001 &&
911                                 !capable(CAP_NET_BIND_SERVICE)) {
912                 err = -EACCES;
913                 goto done;
914         }
915
916         write_lock_bh(&l2cap_sk_list.lock);
917
918         if (la.l2_psm && __l2cap_get_sock_by_addr(la.l2_psm, &la.l2_bdaddr)) {
919                 err = -EADDRINUSE;
920         } else {
921                 /* Save source address */
922                 bacpy(&bt_sk(sk)->src, &la.l2_bdaddr);
923                 l2cap_pi(sk)->psm   = la.l2_psm;
924                 l2cap_pi(sk)->sport = la.l2_psm;
925                 sk->sk_state = BT_BOUND;
926
927                 if (__le16_to_cpu(la.l2_psm) == 0x0001 ||
928                                         __le16_to_cpu(la.l2_psm) == 0x0003)
929                         l2cap_pi(sk)->sec_level = BT_SECURITY_SDP;
930         }
931
932         write_unlock_bh(&l2cap_sk_list.lock);
933
934 done:
935         release_sock(sk);
936         return err;
937 }
938
939 static int l2cap_do_connect(struct sock *sk)
940 {
941         bdaddr_t *src = &bt_sk(sk)->src;
942         bdaddr_t *dst = &bt_sk(sk)->dst;
943         struct l2cap_conn *conn;
944         struct hci_conn *hcon;
945         struct hci_dev *hdev;
946         __u8 auth_type;
947         int err;
948
949         BT_DBG("%s -> %s psm 0x%2.2x", batostr(src), batostr(dst),
950                                                         l2cap_pi(sk)->psm);
951
952         hdev = hci_get_route(dst, src);
953         if (!hdev)
954                 return -EHOSTUNREACH;
955
956         hci_dev_lock_bh(hdev);
957
958         err = -ENOMEM;
959
960         if (sk->sk_type == SOCK_RAW) {
961                 switch (l2cap_pi(sk)->sec_level) {
962                 case BT_SECURITY_HIGH:
963                         auth_type = HCI_AT_DEDICATED_BONDING_MITM;
964                         break;
965                 case BT_SECURITY_MEDIUM:
966                         auth_type = HCI_AT_DEDICATED_BONDING;
967                         break;
968                 default:
969                         auth_type = HCI_AT_NO_BONDING;
970                         break;
971                 }
972         } else if (l2cap_pi(sk)->psm == cpu_to_le16(0x0001)) {
973                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_HIGH)
974                         auth_type = HCI_AT_NO_BONDING_MITM;
975                 else
976                         auth_type = HCI_AT_NO_BONDING;
977
978                 if (l2cap_pi(sk)->sec_level == BT_SECURITY_LOW)
979                         l2cap_pi(sk)->sec_level = BT_SECURITY_SDP;
980         } else {
981                 switch (l2cap_pi(sk)->sec_level) {
982                 case BT_SECURITY_HIGH:
983                         auth_type = HCI_AT_GENERAL_BONDING_MITM;
984                         break;
985                 case BT_SECURITY_MEDIUM:
986                         auth_type = HCI_AT_GENERAL_BONDING;
987                         break;
988                 default:
989                         auth_type = HCI_AT_NO_BONDING;
990                         break;
991                 }
992         }
993
994         hcon = hci_connect(hdev, ACL_LINK, dst,
995                                         l2cap_pi(sk)->sec_level, auth_type);
996         if (!hcon)
997                 goto done;
998
999         conn = l2cap_conn_add(hcon, 0);
1000         if (!conn) {
1001                 hci_conn_put(hcon);
1002                 goto done;
1003         }
1004
1005         err = 0;
1006
1007         /* Update source addr of the socket */
1008         bacpy(src, conn->src);
1009
1010         l2cap_chan_add(conn, sk, NULL);
1011
1012         sk->sk_state = BT_CONNECT;
1013         l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
1014
1015         if (hcon->state == BT_CONNECTED) {
1016                 if (sk->sk_type != SOCK_SEQPACKET &&
1017                                 sk->sk_type != SOCK_STREAM) {
1018                         l2cap_sock_clear_timer(sk);
1019                         sk->sk_state = BT_CONNECTED;
1020                 } else
1021                         l2cap_do_start(sk);
1022         }
1023
1024 done:
1025         hci_dev_unlock_bh(hdev);
1026         hci_dev_put(hdev);
1027         return err;
1028 }
1029
1030 static int l2cap_sock_connect(struct socket *sock, struct sockaddr *addr, int alen, int flags)
1031 {
1032         struct sock *sk = sock->sk;
1033         struct sockaddr_l2 la;
1034         int len, err = 0;
1035
1036         BT_DBG("sk %p", sk);
1037
1038         if (!addr || alen < sizeof(addr->sa_family) ||
1039             addr->sa_family != AF_BLUETOOTH)
1040                 return -EINVAL;
1041
1042         memset(&la, 0, sizeof(la));
1043         len = min_t(unsigned int, sizeof(la), alen);
1044         memcpy(&la, addr, len);
1045
1046         if (la.l2_cid)
1047                 return -EINVAL;
1048
1049         lock_sock(sk);
1050
1051         if ((sk->sk_type == SOCK_SEQPACKET || sk->sk_type == SOCK_STREAM)
1052                         && !la.l2_psm) {
1053                 err = -EINVAL;
1054                 goto done;
1055         }
1056
1057         switch (l2cap_pi(sk)->mode) {
1058         case L2CAP_MODE_BASIC:
1059                 break;
1060         case L2CAP_MODE_ERTM:
1061         case L2CAP_MODE_STREAMING:
1062                 if (enable_ertm)
1063                         break;
1064                 /* fall through */
1065         default:
1066                 err = -ENOTSUPP;
1067                 goto done;
1068         }
1069
1070         switch (sk->sk_state) {
1071         case BT_CONNECT:
1072         case BT_CONNECT2:
1073         case BT_CONFIG:
1074                 /* Already connecting */
1075                 goto wait;
1076
1077         case BT_CONNECTED:
1078                 /* Already connected */
1079                 goto done;
1080
1081         case BT_OPEN:
1082         case BT_BOUND:
1083                 /* Can connect */
1084                 break;
1085
1086         default:
1087                 err = -EBADFD;
1088                 goto done;
1089         }
1090
1091         /* Set destination address and psm */
1092         bacpy(&bt_sk(sk)->dst, &la.l2_bdaddr);
1093         l2cap_pi(sk)->psm = la.l2_psm;
1094
1095         err = l2cap_do_connect(sk);
1096         if (err)
1097                 goto done;
1098
1099 wait:
1100         err = bt_sock_wait_state(sk, BT_CONNECTED,
1101                         sock_sndtimeo(sk, flags & O_NONBLOCK));
1102 done:
1103         release_sock(sk);
1104         return err;
1105 }
1106
1107 static int l2cap_sock_listen(struct socket *sock, int backlog)
1108 {
1109         struct sock *sk = sock->sk;
1110         int err = 0;
1111
1112         BT_DBG("sk %p backlog %d", sk, backlog);
1113
1114         lock_sock(sk);
1115
1116         if ((sock->type != SOCK_SEQPACKET && sock->type != SOCK_STREAM)
1117                         || sk->sk_state != BT_BOUND) {
1118                 err = -EBADFD;
1119                 goto done;
1120         }
1121
1122         switch (l2cap_pi(sk)->mode) {
1123         case L2CAP_MODE_BASIC:
1124                 break;
1125         case L2CAP_MODE_ERTM:
1126         case L2CAP_MODE_STREAMING:
1127                 if (enable_ertm)
1128                         break;
1129                 /* fall through */
1130         default:
1131                 err = -ENOTSUPP;
1132                 goto done;
1133         }
1134
1135         if (!l2cap_pi(sk)->psm) {
1136                 bdaddr_t *src = &bt_sk(sk)->src;
1137                 u16 psm;
1138
1139                 err = -EINVAL;
1140
1141                 write_lock_bh(&l2cap_sk_list.lock);
1142
1143                 for (psm = 0x1001; psm < 0x1100; psm += 2)
1144                         if (!__l2cap_get_sock_by_addr(cpu_to_le16(psm), src)) {
1145                                 l2cap_pi(sk)->psm   = cpu_to_le16(psm);
1146                                 l2cap_pi(sk)->sport = cpu_to_le16(psm);
1147                                 err = 0;
1148                                 break;
1149                         }
1150
1151                 write_unlock_bh(&l2cap_sk_list.lock);
1152
1153                 if (err < 0)
1154                         goto done;
1155         }
1156
1157         sk->sk_max_ack_backlog = backlog;
1158         sk->sk_ack_backlog = 0;
1159         sk->sk_state = BT_LISTEN;
1160
1161 done:
1162         release_sock(sk);
1163         return err;
1164 }
1165
1166 static int l2cap_sock_accept(struct socket *sock, struct socket *newsock, int flags)
1167 {
1168         DECLARE_WAITQUEUE(wait, current);
1169         struct sock *sk = sock->sk, *nsk;
1170         long timeo;
1171         int err = 0;
1172
1173         lock_sock_nested(sk, SINGLE_DEPTH_NESTING);
1174
1175         if (sk->sk_state != BT_LISTEN) {
1176                 err = -EBADFD;
1177                 goto done;
1178         }
1179
1180         timeo = sock_rcvtimeo(sk, flags & O_NONBLOCK);
1181
1182         BT_DBG("sk %p timeo %ld", sk, timeo);
1183
1184         /* Wait for an incoming connection. (wake-one). */
1185         add_wait_queue_exclusive(sk_sleep(sk), &wait);
1186         while (!(nsk = bt_accept_dequeue(sk, newsock))) {
1187                 set_current_state(TASK_INTERRUPTIBLE);
1188                 if (!timeo) {
1189                         err = -EAGAIN;
1190                         break;
1191                 }
1192
1193                 release_sock(sk);
1194                 timeo = schedule_timeout(timeo);
1195                 lock_sock_nested(sk, SINGLE_DEPTH_NESTING);
1196
1197                 if (sk->sk_state != BT_LISTEN) {
1198                         err = -EBADFD;
1199                         break;
1200                 }
1201
1202                 if (signal_pending(current)) {
1203                         err = sock_intr_errno(timeo);
1204                         break;
1205                 }
1206         }
1207         set_current_state(TASK_RUNNING);
1208         remove_wait_queue(sk_sleep(sk), &wait);
1209
1210         if (err)
1211                 goto done;
1212
1213         newsock->state = SS_CONNECTED;
1214
1215         BT_DBG("new socket %p", nsk);
1216
1217 done:
1218         release_sock(sk);
1219         return err;
1220 }
1221
1222 static int l2cap_sock_getname(struct socket *sock, struct sockaddr *addr, int *len, int peer)
1223 {
1224         struct sockaddr_l2 *la = (struct sockaddr_l2 *) addr;
1225         struct sock *sk = sock->sk;
1226
1227         BT_DBG("sock %p, sk %p", sock, sk);
1228
1229         addr->sa_family = AF_BLUETOOTH;
1230         *len = sizeof(struct sockaddr_l2);
1231
1232         if (peer) {
1233                 la->l2_psm = l2cap_pi(sk)->psm;
1234                 bacpy(&la->l2_bdaddr, &bt_sk(sk)->dst);
1235                 la->l2_cid = cpu_to_le16(l2cap_pi(sk)->dcid);
1236         } else {
1237                 la->l2_psm = l2cap_pi(sk)->sport;
1238                 bacpy(&la->l2_bdaddr, &bt_sk(sk)->src);
1239                 la->l2_cid = cpu_to_le16(l2cap_pi(sk)->scid);
1240         }
1241
1242         return 0;
1243 }
1244
1245 static int __l2cap_wait_ack(struct sock *sk)
1246 {
1247         DECLARE_WAITQUEUE(wait, current);
1248         int err = 0;
1249         int timeo = HZ/5;
1250
1251         add_wait_queue(sk_sleep(sk), &wait);
1252         while ((l2cap_pi(sk)->unacked_frames > 0 && l2cap_pi(sk)->conn)) {
1253                 set_current_state(TASK_INTERRUPTIBLE);
1254
1255                 if (!timeo)
1256                         timeo = HZ/5;
1257
1258                 if (signal_pending(current)) {
1259                         err = sock_intr_errno(timeo);
1260                         break;
1261                 }
1262
1263                 release_sock(sk);
1264                 timeo = schedule_timeout(timeo);
1265                 lock_sock(sk);
1266
1267                 err = sock_error(sk);
1268                 if (err)
1269                         break;
1270         }
1271         set_current_state(TASK_RUNNING);
1272         remove_wait_queue(sk_sleep(sk), &wait);
1273         return err;
1274 }
1275
1276 static void l2cap_monitor_timeout(unsigned long arg)
1277 {
1278         struct sock *sk = (void *) arg;
1279
1280         bh_lock_sock(sk);
1281         if (l2cap_pi(sk)->retry_count >= l2cap_pi(sk)->remote_max_tx) {
1282                 l2cap_send_disconn_req(l2cap_pi(sk)->conn, sk);
1283                 bh_unlock_sock(sk);
1284                 return;
1285         }
1286
1287         l2cap_pi(sk)->retry_count++;
1288         __mod_monitor_timer();
1289
1290         l2cap_send_rr_or_rnr(l2cap_pi(sk), L2CAP_CTRL_POLL);
1291         bh_unlock_sock(sk);
1292 }
1293
1294 static void l2cap_retrans_timeout(unsigned long arg)
1295 {
1296         struct sock *sk = (void *) arg;
1297
1298         bh_lock_sock(sk);
1299         l2cap_pi(sk)->retry_count = 1;
1300         __mod_monitor_timer();
1301
1302         l2cap_pi(sk)->conn_state |= L2CAP_CONN_WAIT_F;
1303
1304         l2cap_send_rr_or_rnr(l2cap_pi(sk), L2CAP_CTRL_POLL);
1305         bh_unlock_sock(sk);
1306 }
1307
1308 static void l2cap_drop_acked_frames(struct sock *sk)
1309 {
1310         struct sk_buff *skb;
1311
1312         while ((skb = skb_peek(TX_QUEUE(sk))) &&
1313                         l2cap_pi(sk)->unacked_frames) {
1314                 if (bt_cb(skb)->tx_seq == l2cap_pi(sk)->expected_ack_seq)
1315                         break;
1316
1317                 skb = skb_dequeue(TX_QUEUE(sk));
1318                 kfree_skb(skb);
1319
1320                 l2cap_pi(sk)->unacked_frames--;
1321         }
1322
1323         if (!l2cap_pi(sk)->unacked_frames)
1324                 del_timer(&l2cap_pi(sk)->retrans_timer);
1325
1326         return;
1327 }
1328
1329 static inline void l2cap_do_send(struct sock *sk, struct sk_buff *skb)
1330 {
1331         struct l2cap_pinfo *pi = l2cap_pi(sk);
1332
1333         BT_DBG("sk %p, skb %p len %d", sk, skb, skb->len);
1334
1335         hci_send_acl(pi->conn->hcon, skb, 0);
1336 }
1337
1338 static int l2cap_streaming_send(struct sock *sk)
1339 {
1340         struct sk_buff *skb, *tx_skb;
1341         struct l2cap_pinfo *pi = l2cap_pi(sk);
1342         u16 control, fcs;
1343
1344         while ((skb = sk->sk_send_head)) {
1345                 tx_skb = skb_clone(skb, GFP_ATOMIC);
1346
1347                 control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE);
1348                 control |= pi->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT;
1349                 put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE);
1350
1351                 if (pi->fcs == L2CAP_FCS_CRC16) {
1352                         fcs = crc16(0, (u8 *)tx_skb->data, tx_skb->len - 2);
1353                         put_unaligned_le16(fcs, tx_skb->data + tx_skb->len - 2);
1354                 }
1355
1356                 l2cap_do_send(sk, tx_skb);
1357
1358                 pi->next_tx_seq = (pi->next_tx_seq + 1) % 64;
1359
1360                 if (skb_queue_is_last(TX_QUEUE(sk), skb))
1361                         sk->sk_send_head = NULL;
1362                 else
1363                         sk->sk_send_head = skb_queue_next(TX_QUEUE(sk), skb);
1364
1365                 skb = skb_dequeue(TX_QUEUE(sk));
1366                 kfree_skb(skb);
1367         }
1368         return 0;
1369 }
1370
1371 static void l2cap_retransmit_one_frame(struct sock *sk, u8 tx_seq)
1372 {
1373         struct l2cap_pinfo *pi = l2cap_pi(sk);
1374         struct sk_buff *skb, *tx_skb;
1375         u16 control, fcs;
1376
1377         skb = skb_peek(TX_QUEUE(sk));
1378         if (!skb)
1379                 return;
1380
1381         do {
1382                 if (bt_cb(skb)->tx_seq == tx_seq)
1383                         break;
1384
1385                 if (skb_queue_is_last(TX_QUEUE(sk), skb))
1386                         return;
1387
1388         } while ((skb = skb_queue_next(TX_QUEUE(sk), skb)));
1389
1390         if (pi->remote_max_tx &&
1391                         bt_cb(skb)->retries == pi->remote_max_tx) {
1392                 l2cap_send_disconn_req(pi->conn, sk);
1393                 return;
1394         }
1395
1396         tx_skb = skb_clone(skb, GFP_ATOMIC);
1397         bt_cb(skb)->retries++;
1398         control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE);
1399         control |= (pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT)
1400                         | (tx_seq << L2CAP_CTRL_TXSEQ_SHIFT);
1401         put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE);
1402
1403         if (pi->fcs == L2CAP_FCS_CRC16) {
1404                 fcs = crc16(0, (u8 *)tx_skb->data, tx_skb->len - 2);
1405                 put_unaligned_le16(fcs, tx_skb->data + tx_skb->len - 2);
1406         }
1407
1408         l2cap_do_send(sk, tx_skb);
1409 }
1410
1411 static int l2cap_ertm_send(struct sock *sk)
1412 {
1413         struct sk_buff *skb, *tx_skb;
1414         struct l2cap_pinfo *pi = l2cap_pi(sk);
1415         u16 control, fcs;
1416         int nsent = 0;
1417
1418         if (pi->conn_state & L2CAP_CONN_WAIT_F)
1419                 return 0;
1420
1421         while ((skb = sk->sk_send_head) && (!l2cap_tx_window_full(sk)) &&
1422                         !(pi->conn_state & L2CAP_CONN_REMOTE_BUSY)) {
1423
1424                 if (pi->remote_max_tx &&
1425                                 bt_cb(skb)->retries == pi->remote_max_tx) {
1426                         l2cap_send_disconn_req(pi->conn, sk);
1427                         break;
1428                 }
1429
1430                 tx_skb = skb_clone(skb, GFP_ATOMIC);
1431
1432                 bt_cb(skb)->retries++;
1433
1434                 control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE);
1435                 if (pi->conn_state & L2CAP_CONN_SEND_FBIT) {
1436                         control |= L2CAP_CTRL_FINAL;
1437                         pi->conn_state &= ~L2CAP_CONN_SEND_FBIT;
1438                 }
1439                 control |= (pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT)
1440                                 | (pi->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT);
1441                 put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE);
1442
1443
1444                 if (pi->fcs == L2CAP_FCS_CRC16) {
1445                         fcs = crc16(0, (u8 *)skb->data, tx_skb->len - 2);
1446                         put_unaligned_le16(fcs, skb->data + tx_skb->len - 2);
1447                 }
1448
1449                 l2cap_do_send(sk, tx_skb);
1450
1451                 __mod_retrans_timer();
1452
1453                 bt_cb(skb)->tx_seq = pi->next_tx_seq;
1454                 pi->next_tx_seq = (pi->next_tx_seq + 1) % 64;
1455
1456                 pi->unacked_frames++;
1457                 pi->frames_sent++;
1458
1459                 if (skb_queue_is_last(TX_QUEUE(sk), skb))
1460                         sk->sk_send_head = NULL;
1461                 else
1462                         sk->sk_send_head = skb_queue_next(TX_QUEUE(sk), skb);
1463
1464                 nsent++;
1465         }
1466
1467         return nsent;
1468 }
1469
1470 static int l2cap_retransmit_frames(struct sock *sk)
1471 {
1472         struct l2cap_pinfo *pi = l2cap_pi(sk);
1473         int ret;
1474
1475         spin_lock_bh(&pi->send_lock);
1476
1477         if (!skb_queue_empty(TX_QUEUE(sk)))
1478                 sk->sk_send_head = TX_QUEUE(sk)->next;
1479
1480         pi->next_tx_seq = pi->expected_ack_seq;
1481         ret = l2cap_ertm_send(sk);
1482
1483         spin_unlock_bh(&pi->send_lock);
1484
1485         return ret;
1486 }
1487
1488 static void l2cap_send_ack(struct l2cap_pinfo *pi)
1489 {
1490         struct sock *sk = (struct sock *)pi;
1491         u16 control = 0;
1492         int nframes;
1493
1494         control |= pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
1495
1496         if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
1497                 control |= L2CAP_SUPER_RCV_NOT_READY;
1498                 pi->conn_state |= L2CAP_CONN_RNR_SENT;
1499                 l2cap_send_sframe(pi, control);
1500                 return;
1501         }
1502
1503         spin_lock_bh(&pi->send_lock);
1504         nframes = l2cap_ertm_send(sk);
1505         spin_unlock_bh(&pi->send_lock);
1506
1507         if (nframes > 0)
1508                 return;
1509
1510         control |= L2CAP_SUPER_RCV_READY;
1511         l2cap_send_sframe(pi, control);
1512 }
1513
1514 static void l2cap_send_srejtail(struct sock *sk)
1515 {
1516         struct srej_list *tail;
1517         u16 control;
1518
1519         control = L2CAP_SUPER_SELECT_REJECT;
1520         control |= L2CAP_CTRL_FINAL;
1521
1522         tail = list_entry(SREJ_LIST(sk)->prev, struct srej_list, list);
1523         control |= tail->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
1524
1525         l2cap_send_sframe(l2cap_pi(sk), control);
1526 }
1527
1528 static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, int len, int count, struct sk_buff *skb)
1529 {
1530         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1531         struct sk_buff **frag;
1532         int err, sent = 0;
1533
1534         if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count))
1535                 return -EFAULT;
1536
1537         sent += count;
1538         len  -= count;
1539
1540         /* Continuation fragments (no L2CAP header) */
1541         frag = &skb_shinfo(skb)->frag_list;
1542         while (len) {
1543                 count = min_t(unsigned int, conn->mtu, len);
1544
1545                 *frag = bt_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err);
1546                 if (!*frag)
1547                         return -EFAULT;
1548                 if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count))
1549                         return -EFAULT;
1550
1551                 sent += count;
1552                 len  -= count;
1553
1554                 frag = &(*frag)->next;
1555         }
1556
1557         return sent;
1558 }
1559
1560 static struct sk_buff *l2cap_create_connless_pdu(struct sock *sk, struct msghdr *msg, size_t len)
1561 {
1562         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1563         struct sk_buff *skb;
1564         int err, count, hlen = L2CAP_HDR_SIZE + 2;
1565         struct l2cap_hdr *lh;
1566
1567         BT_DBG("sk %p len %d", sk, (int)len);
1568
1569         count = min_t(unsigned int, (conn->mtu - hlen), len);
1570         skb = bt_skb_send_alloc(sk, count + hlen,
1571                         msg->msg_flags & MSG_DONTWAIT, &err);
1572         if (!skb)
1573                 return ERR_PTR(-ENOMEM);
1574
1575         /* Create L2CAP header */
1576         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1577         lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
1578         lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1579         put_unaligned_le16(l2cap_pi(sk)->psm, skb_put(skb, 2));
1580
1581         err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1582         if (unlikely(err < 0)) {
1583                 kfree_skb(skb);
1584                 return ERR_PTR(err);
1585         }
1586         return skb;
1587 }
1588
1589 static struct sk_buff *l2cap_create_basic_pdu(struct sock *sk, struct msghdr *msg, size_t len)
1590 {
1591         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1592         struct sk_buff *skb;
1593         int err, count, hlen = L2CAP_HDR_SIZE;
1594         struct l2cap_hdr *lh;
1595
1596         BT_DBG("sk %p len %d", sk, (int)len);
1597
1598         count = min_t(unsigned int, (conn->mtu - hlen), len);
1599         skb = bt_skb_send_alloc(sk, count + hlen,
1600                         msg->msg_flags & MSG_DONTWAIT, &err);
1601         if (!skb)
1602                 return ERR_PTR(-ENOMEM);
1603
1604         /* Create L2CAP header */
1605         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1606         lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
1607         lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1608
1609         err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1610         if (unlikely(err < 0)) {
1611                 kfree_skb(skb);
1612                 return ERR_PTR(err);
1613         }
1614         return skb;
1615 }
1616
1617 static struct sk_buff *l2cap_create_iframe_pdu(struct sock *sk, struct msghdr *msg, size_t len, u16 control, u16 sdulen)
1618 {
1619         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1620         struct sk_buff *skb;
1621         int err, count, hlen = L2CAP_HDR_SIZE + 2;
1622         struct l2cap_hdr *lh;
1623
1624         BT_DBG("sk %p len %d", sk, (int)len);
1625
1626         if (!conn)
1627                 return ERR_PTR(-ENOTCONN);
1628
1629         if (sdulen)
1630                 hlen += 2;
1631
1632         if (l2cap_pi(sk)->fcs == L2CAP_FCS_CRC16)
1633                 hlen += 2;
1634
1635         count = min_t(unsigned int, (conn->mtu - hlen), len);
1636         skb = bt_skb_send_alloc(sk, count + hlen,
1637                         msg->msg_flags & MSG_DONTWAIT, &err);
1638         if (!skb)
1639                 return ERR_PTR(-ENOMEM);
1640
1641         /* Create L2CAP header */
1642         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1643         lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
1644         lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1645         put_unaligned_le16(control, skb_put(skb, 2));
1646         if (sdulen)
1647                 put_unaligned_le16(sdulen, skb_put(skb, 2));
1648
1649         err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1650         if (unlikely(err < 0)) {
1651                 kfree_skb(skb);
1652                 return ERR_PTR(err);
1653         }
1654
1655         if (l2cap_pi(sk)->fcs == L2CAP_FCS_CRC16)
1656                 put_unaligned_le16(0, skb_put(skb, 2));
1657
1658         bt_cb(skb)->retries = 0;
1659         return skb;
1660 }
1661
1662 static inline int l2cap_sar_segment_sdu(struct sock *sk, struct msghdr *msg, size_t len)
1663 {
1664         struct l2cap_pinfo *pi = l2cap_pi(sk);
1665         struct sk_buff *skb;
1666         struct sk_buff_head sar_queue;
1667         u16 control;
1668         size_t size = 0;
1669
1670         skb_queue_head_init(&sar_queue);
1671         control = L2CAP_SDU_START;
1672         skb = l2cap_create_iframe_pdu(sk, msg, pi->remote_mps, control, len);
1673         if (IS_ERR(skb))
1674                 return PTR_ERR(skb);
1675
1676         __skb_queue_tail(&sar_queue, skb);
1677         len -= pi->remote_mps;
1678         size += pi->remote_mps;
1679
1680         while (len > 0) {
1681                 size_t buflen;
1682
1683                 if (len > pi->remote_mps) {
1684                         control = L2CAP_SDU_CONTINUE;
1685                         buflen = pi->remote_mps;
1686                 } else {
1687                         control = L2CAP_SDU_END;
1688                         buflen = len;
1689                 }
1690
1691                 skb = l2cap_create_iframe_pdu(sk, msg, buflen, control, 0);
1692                 if (IS_ERR(skb)) {
1693                         skb_queue_purge(&sar_queue);
1694                         return PTR_ERR(skb);
1695                 }
1696
1697                 __skb_queue_tail(&sar_queue, skb);
1698                 len -= buflen;
1699                 size += buflen;
1700         }
1701         skb_queue_splice_tail(&sar_queue, TX_QUEUE(sk));
1702         spin_lock_bh(&pi->send_lock);
1703         if (sk->sk_send_head == NULL)
1704                 sk->sk_send_head = sar_queue.next;
1705         spin_unlock_bh(&pi->send_lock);
1706
1707         return size;
1708 }
1709
1710 static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len)
1711 {
1712         struct sock *sk = sock->sk;
1713         struct l2cap_pinfo *pi = l2cap_pi(sk);
1714         struct sk_buff *skb;
1715         u16 control;
1716         int err;
1717
1718         BT_DBG("sock %p, sk %p", sock, sk);
1719
1720         err = sock_error(sk);
1721         if (err)
1722                 return err;
1723
1724         if (msg->msg_flags & MSG_OOB)
1725                 return -EOPNOTSUPP;
1726
1727         lock_sock(sk);
1728
1729         if (sk->sk_state != BT_CONNECTED) {
1730                 err = -ENOTCONN;
1731                 goto done;
1732         }
1733
1734         /* Connectionless channel */
1735         if (sk->sk_type == SOCK_DGRAM) {
1736                 skb = l2cap_create_connless_pdu(sk, msg, len);
1737                 if (IS_ERR(skb)) {
1738                         err = PTR_ERR(skb);
1739                 } else {
1740                         l2cap_do_send(sk, skb);
1741                         err = len;
1742                 }
1743                 goto done;
1744         }
1745
1746         switch (pi->mode) {
1747         case L2CAP_MODE_BASIC:
1748                 /* Check outgoing MTU */
1749                 if (len > pi->omtu) {
1750                         err = -EINVAL;
1751                         goto done;
1752                 }
1753
1754                 /* Create a basic PDU */
1755                 skb = l2cap_create_basic_pdu(sk, msg, len);
1756                 if (IS_ERR(skb)) {
1757                         err = PTR_ERR(skb);
1758                         goto done;
1759                 }
1760
1761                 l2cap_do_send(sk, skb);
1762                 err = len;
1763                 break;
1764
1765         case L2CAP_MODE_ERTM:
1766         case L2CAP_MODE_STREAMING:
1767                 /* Entire SDU fits into one PDU */
1768                 if (len <= pi->remote_mps) {
1769                         control = L2CAP_SDU_UNSEGMENTED;
1770                         skb = l2cap_create_iframe_pdu(sk, msg, len, control, 0);
1771                         if (IS_ERR(skb)) {
1772                                 err = PTR_ERR(skb);
1773                                 goto done;
1774                         }
1775                         __skb_queue_tail(TX_QUEUE(sk), skb);
1776
1777                         if (pi->mode == L2CAP_MODE_ERTM)
1778                                 spin_lock_bh(&pi->send_lock);
1779
1780                         if (sk->sk_send_head == NULL)
1781                                 sk->sk_send_head = skb;
1782
1783                         if (pi->mode == L2CAP_MODE_ERTM)
1784                                 spin_unlock_bh(&pi->send_lock);
1785                 } else {
1786                 /* Segment SDU into multiples PDUs */
1787                         err = l2cap_sar_segment_sdu(sk, msg, len);
1788                         if (err < 0)
1789                                 goto done;
1790                 }
1791
1792                 if (pi->mode == L2CAP_MODE_STREAMING) {
1793                         err = l2cap_streaming_send(sk);
1794                 } else {
1795                         spin_lock_bh(&pi->send_lock);
1796                         err = l2cap_ertm_send(sk);
1797                         spin_unlock_bh(&pi->send_lock);
1798                 }
1799
1800                 if (err >= 0)
1801                         err = len;
1802                 break;
1803
1804         default:
1805                 BT_DBG("bad state %1.1x", pi->mode);
1806                 err = -EINVAL;
1807         }
1808
1809 done:
1810         release_sock(sk);
1811         return err;
1812 }
1813
1814 static int l2cap_sock_recvmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len, int flags)
1815 {
1816         struct sock *sk = sock->sk;
1817
1818         lock_sock(sk);
1819
1820         if (sk->sk_state == BT_CONNECT2 && bt_sk(sk)->defer_setup) {
1821                 struct l2cap_conn_rsp rsp;
1822
1823                 sk->sk_state = BT_CONFIG;
1824
1825                 rsp.scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
1826                 rsp.dcid   = cpu_to_le16(l2cap_pi(sk)->scid);
1827                 rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
1828                 rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
1829                 l2cap_send_cmd(l2cap_pi(sk)->conn, l2cap_pi(sk)->ident,
1830                                         L2CAP_CONN_RSP, sizeof(rsp), &rsp);
1831
1832                 release_sock(sk);
1833                 return 0;
1834         }
1835
1836         release_sock(sk);
1837
1838         return bt_sock_recvmsg(iocb, sock, msg, len, flags);
1839 }
1840
1841 static int l2cap_sock_setsockopt_old(struct socket *sock, int optname, char __user *optval, unsigned int optlen)
1842 {
1843         struct sock *sk = sock->sk;
1844         struct l2cap_options opts;
1845         int len, err = 0;
1846         u32 opt;
1847
1848         BT_DBG("sk %p", sk);
1849
1850         lock_sock(sk);
1851
1852         switch (optname) {
1853         case L2CAP_OPTIONS:
1854                 opts.imtu     = l2cap_pi(sk)->imtu;
1855                 opts.omtu     = l2cap_pi(sk)->omtu;
1856                 opts.flush_to = l2cap_pi(sk)->flush_to;
1857                 opts.mode     = l2cap_pi(sk)->mode;
1858                 opts.fcs      = l2cap_pi(sk)->fcs;
1859                 opts.max_tx   = l2cap_pi(sk)->max_tx;
1860                 opts.txwin_size = (__u16)l2cap_pi(sk)->tx_win;
1861
1862                 len = min_t(unsigned int, sizeof(opts), optlen);
1863                 if (copy_from_user((char *) &opts, optval, len)) {
1864                         err = -EFAULT;
1865                         break;
1866                 }
1867
1868                 l2cap_pi(sk)->mode = opts.mode;
1869                 switch (l2cap_pi(sk)->mode) {
1870                 case L2CAP_MODE_BASIC:
1871                         break;
1872                 case L2CAP_MODE_ERTM:
1873                 case L2CAP_MODE_STREAMING:
1874                         if (enable_ertm)
1875                                 break;
1876                         /* fall through */
1877                 default:
1878                         err = -EINVAL;
1879                         break;
1880                 }
1881
1882                 l2cap_pi(sk)->imtu = opts.imtu;
1883                 l2cap_pi(sk)->omtu = opts.omtu;
1884                 l2cap_pi(sk)->fcs  = opts.fcs;
1885                 l2cap_pi(sk)->max_tx = opts.max_tx;
1886                 l2cap_pi(sk)->tx_win = (__u8)opts.txwin_size;
1887                 break;
1888
1889         case L2CAP_LM:
1890                 if (get_user(opt, (u32 __user *) optval)) {
1891                         err = -EFAULT;
1892                         break;
1893                 }
1894
1895                 if (opt & L2CAP_LM_AUTH)
1896                         l2cap_pi(sk)->sec_level = BT_SECURITY_LOW;
1897                 if (opt & L2CAP_LM_ENCRYPT)
1898                         l2cap_pi(sk)->sec_level = BT_SECURITY_MEDIUM;
1899                 if (opt & L2CAP_LM_SECURE)
1900                         l2cap_pi(sk)->sec_level = BT_SECURITY_HIGH;
1901
1902                 l2cap_pi(sk)->role_switch    = (opt & L2CAP_LM_MASTER);
1903                 l2cap_pi(sk)->force_reliable = (opt & L2CAP_LM_RELIABLE);
1904                 break;
1905
1906         default:
1907                 err = -ENOPROTOOPT;
1908                 break;
1909         }
1910
1911         release_sock(sk);
1912         return err;
1913 }
1914
1915 static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname, char __user *optval, unsigned int optlen)
1916 {
1917         struct sock *sk = sock->sk;
1918         struct bt_security sec;
1919         int len, err = 0;
1920         u32 opt;
1921
1922         BT_DBG("sk %p", sk);
1923
1924         if (level == SOL_L2CAP)
1925                 return l2cap_sock_setsockopt_old(sock, optname, optval, optlen);
1926
1927         if (level != SOL_BLUETOOTH)
1928                 return -ENOPROTOOPT;
1929
1930         lock_sock(sk);
1931
1932         switch (optname) {
1933         case BT_SECURITY:
1934                 if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM
1935                                 && sk->sk_type != SOCK_RAW) {
1936                         err = -EINVAL;
1937                         break;
1938                 }
1939
1940                 sec.level = BT_SECURITY_LOW;
1941
1942                 len = min_t(unsigned int, sizeof(sec), optlen);
1943                 if (copy_from_user((char *) &sec, optval, len)) {
1944                         err = -EFAULT;
1945                         break;
1946                 }
1947
1948                 if (sec.level < BT_SECURITY_LOW ||
1949                                         sec.level > BT_SECURITY_HIGH) {
1950                         err = -EINVAL;
1951                         break;
1952                 }
1953
1954                 l2cap_pi(sk)->sec_level = sec.level;
1955                 break;
1956
1957         case BT_DEFER_SETUP:
1958                 if (sk->sk_state != BT_BOUND && sk->sk_state != BT_LISTEN) {
1959                         err = -EINVAL;
1960                         break;
1961                 }
1962
1963                 if (get_user(opt, (u32 __user *) optval)) {
1964                         err = -EFAULT;
1965                         break;
1966                 }
1967
1968                 bt_sk(sk)->defer_setup = opt;
1969                 break;
1970
1971         default:
1972                 err = -ENOPROTOOPT;
1973                 break;
1974         }
1975
1976         release_sock(sk);
1977         return err;
1978 }
1979
1980 static int l2cap_sock_getsockopt_old(struct socket *sock, int optname, char __user *optval, int __user *optlen)
1981 {
1982         struct sock *sk = sock->sk;
1983         struct l2cap_options opts;
1984         struct l2cap_conninfo cinfo;
1985         int len, err = 0;
1986         u32 opt;
1987
1988         BT_DBG("sk %p", sk);
1989
1990         if (get_user(len, optlen))
1991                 return -EFAULT;
1992
1993         lock_sock(sk);
1994
1995         switch (optname) {
1996         case L2CAP_OPTIONS:
1997                 opts.imtu     = l2cap_pi(sk)->imtu;
1998                 opts.omtu     = l2cap_pi(sk)->omtu;
1999                 opts.flush_to = l2cap_pi(sk)->flush_to;
2000                 opts.mode     = l2cap_pi(sk)->mode;
2001                 opts.fcs      = l2cap_pi(sk)->fcs;
2002                 opts.max_tx   = l2cap_pi(sk)->max_tx;
2003                 opts.txwin_size = (__u16)l2cap_pi(sk)->tx_win;
2004
2005                 len = min_t(unsigned int, len, sizeof(opts));
2006                 if (copy_to_user(optval, (char *) &opts, len))
2007                         err = -EFAULT;
2008
2009                 break;
2010
2011         case L2CAP_LM:
2012                 switch (l2cap_pi(sk)->sec_level) {
2013                 case BT_SECURITY_LOW:
2014                         opt = L2CAP_LM_AUTH;
2015                         break;
2016                 case BT_SECURITY_MEDIUM:
2017                         opt = L2CAP_LM_AUTH | L2CAP_LM_ENCRYPT;
2018                         break;
2019                 case BT_SECURITY_HIGH:
2020                         opt = L2CAP_LM_AUTH | L2CAP_LM_ENCRYPT |
2021                                                         L2CAP_LM_SECURE;
2022                         break;
2023                 default:
2024                         opt = 0;
2025                         break;
2026                 }
2027
2028                 if (l2cap_pi(sk)->role_switch)
2029                         opt |= L2CAP_LM_MASTER;
2030
2031                 if (l2cap_pi(sk)->force_reliable)
2032                         opt |= L2CAP_LM_RELIABLE;
2033
2034                 if (put_user(opt, (u32 __user *) optval))
2035                         err = -EFAULT;
2036                 break;
2037
2038         case L2CAP_CONNINFO:
2039                 if (sk->sk_state != BT_CONNECTED &&
2040                                         !(sk->sk_state == BT_CONNECT2 &&
2041                                                 bt_sk(sk)->defer_setup)) {
2042                         err = -ENOTCONN;
2043                         break;
2044                 }
2045
2046                 cinfo.hci_handle = l2cap_pi(sk)->conn->hcon->handle;
2047                 memcpy(cinfo.dev_class, l2cap_pi(sk)->conn->hcon->dev_class, 3);
2048
2049                 len = min_t(unsigned int, len, sizeof(cinfo));
2050                 if (copy_to_user(optval, (char *) &cinfo, len))
2051                         err = -EFAULT;
2052
2053                 break;
2054
2055         default:
2056                 err = -ENOPROTOOPT;
2057                 break;
2058         }
2059
2060         release_sock(sk);
2061         return err;
2062 }
2063
2064 static int l2cap_sock_getsockopt(struct socket *sock, int level, int optname, char __user *optval, int __user *optlen)
2065 {
2066         struct sock *sk = sock->sk;
2067         struct bt_security sec;
2068         int len, err = 0;
2069
2070         BT_DBG("sk %p", sk);
2071
2072         if (level == SOL_L2CAP)
2073                 return l2cap_sock_getsockopt_old(sock, optname, optval, optlen);
2074
2075         if (level != SOL_BLUETOOTH)
2076                 return -ENOPROTOOPT;
2077
2078         if (get_user(len, optlen))
2079                 return -EFAULT;
2080
2081         lock_sock(sk);
2082
2083         switch (optname) {
2084         case BT_SECURITY:
2085                 if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM
2086                                 && sk->sk_type != SOCK_RAW) {
2087                         err = -EINVAL;
2088                         break;
2089                 }
2090
2091                 sec.level = l2cap_pi(sk)->sec_level;
2092
2093                 len = min_t(unsigned int, len, sizeof(sec));
2094                 if (copy_to_user(optval, (char *) &sec, len))
2095                         err = -EFAULT;
2096
2097                 break;
2098
2099         case BT_DEFER_SETUP:
2100                 if (sk->sk_state != BT_BOUND && sk->sk_state != BT_LISTEN) {
2101                         err = -EINVAL;
2102                         break;
2103                 }
2104
2105                 if (put_user(bt_sk(sk)->defer_setup, (u32 __user *) optval))
2106                         err = -EFAULT;
2107
2108                 break;
2109
2110         default:
2111                 err = -ENOPROTOOPT;
2112                 break;
2113         }
2114
2115         release_sock(sk);
2116         return err;
2117 }
2118
2119 static int l2cap_sock_shutdown(struct socket *sock, int how)
2120 {
2121         struct sock *sk = sock->sk;
2122         int err = 0;
2123
2124         BT_DBG("sock %p, sk %p", sock, sk);
2125
2126         if (!sk)
2127                 return 0;
2128
2129         lock_sock(sk);
2130         if (!sk->sk_shutdown) {
2131                 if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM)
2132                         err = __l2cap_wait_ack(sk);
2133
2134                 sk->sk_shutdown = SHUTDOWN_MASK;
2135                 l2cap_sock_clear_timer(sk);
2136                 __l2cap_sock_close(sk, 0);
2137
2138                 if (sock_flag(sk, SOCK_LINGER) && sk->sk_lingertime)
2139                         err = bt_sock_wait_state(sk, BT_CLOSED,
2140                                                         sk->sk_lingertime);
2141         }
2142         release_sock(sk);
2143         return err;
2144 }
2145
2146 static int l2cap_sock_release(struct socket *sock)
2147 {
2148         struct sock *sk = sock->sk;
2149         int err;
2150
2151         BT_DBG("sock %p, sk %p", sock, sk);
2152
2153         if (!sk)
2154                 return 0;
2155
2156         err = l2cap_sock_shutdown(sock, 2);
2157
2158         sock_orphan(sk);
2159         l2cap_sock_kill(sk);
2160         return err;
2161 }
2162
2163 static void l2cap_chan_ready(struct sock *sk)
2164 {
2165         struct sock *parent = bt_sk(sk)->parent;
2166
2167         BT_DBG("sk %p, parent %p", sk, parent);
2168
2169         l2cap_pi(sk)->conf_state = 0;
2170         l2cap_sock_clear_timer(sk);
2171
2172         if (!parent) {
2173                 /* Outgoing channel.
2174                  * Wake up socket sleeping on connect.
2175                  */
2176                 sk->sk_state = BT_CONNECTED;
2177                 sk->sk_state_change(sk);
2178         } else {
2179                 /* Incoming channel.
2180                  * Wake up socket sleeping on accept.
2181                  */
2182                 parent->sk_data_ready(parent, 0);
2183         }
2184 }
2185
2186 /* Copy frame to all raw sockets on that connection */
2187 static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb)
2188 {
2189         struct l2cap_chan_list *l = &conn->chan_list;
2190         struct sk_buff *nskb;
2191         struct sock *sk;
2192
2193         BT_DBG("conn %p", conn);
2194
2195         read_lock(&l->lock);
2196         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
2197                 if (sk->sk_type != SOCK_RAW)
2198                         continue;
2199
2200                 /* Don't send frame to the socket it came from */
2201                 if (skb->sk == sk)
2202                         continue;
2203                 nskb = skb_clone(skb, GFP_ATOMIC);
2204                 if (!nskb)
2205                         continue;
2206
2207                 if (sock_queue_rcv_skb(sk, nskb))
2208                         kfree_skb(nskb);
2209         }
2210         read_unlock(&l->lock);
2211 }
2212
2213 /* ---- L2CAP signalling commands ---- */
2214 static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
2215                                 u8 code, u8 ident, u16 dlen, void *data)
2216 {
2217         struct sk_buff *skb, **frag;
2218         struct l2cap_cmd_hdr *cmd;
2219         struct l2cap_hdr *lh;
2220         int len, count;
2221
2222         BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %d",
2223                         conn, code, ident, dlen);
2224
2225         len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen;
2226         count = min_t(unsigned int, conn->mtu, len);
2227
2228         skb = bt_skb_alloc(count, GFP_ATOMIC);
2229         if (!skb)
2230                 return NULL;
2231
2232         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
2233         lh->len = cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen);
2234         lh->cid = cpu_to_le16(L2CAP_CID_SIGNALING);
2235
2236         cmd = (struct l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE);
2237         cmd->code  = code;
2238         cmd->ident = ident;
2239         cmd->len   = cpu_to_le16(dlen);
2240
2241         if (dlen) {
2242                 count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE;
2243                 memcpy(skb_put(skb, count), data, count);
2244                 data += count;
2245         }
2246
2247         len -= skb->len;
2248
2249         /* Continuation fragments (no L2CAP header) */
2250         frag = &skb_shinfo(skb)->frag_list;
2251         while (len) {
2252                 count = min_t(unsigned int, conn->mtu, len);
2253
2254                 *frag = bt_skb_alloc(count, GFP_ATOMIC);
2255                 if (!*frag)
2256                         goto fail;
2257
2258                 memcpy(skb_put(*frag, count), data, count);
2259
2260                 len  -= count;
2261                 data += count;
2262
2263                 frag = &(*frag)->next;
2264         }
2265
2266         return skb;
2267
2268 fail:
2269         kfree_skb(skb);
2270         return NULL;
2271 }
2272
2273 static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, unsigned long *val)
2274 {
2275         struct l2cap_conf_opt *opt = *ptr;
2276         int len;
2277
2278         len = L2CAP_CONF_OPT_SIZE + opt->len;
2279         *ptr += len;
2280
2281         *type = opt->type;
2282         *olen = opt->len;
2283
2284         switch (opt->len) {
2285         case 1:
2286                 *val = *((u8 *) opt->val);
2287                 break;
2288
2289         case 2:
2290                 *val = __le16_to_cpu(*((__le16 *) opt->val));
2291                 break;
2292
2293         case 4:
2294                 *val = __le32_to_cpu(*((__le32 *) opt->val));
2295                 break;
2296
2297         default:
2298                 *val = (unsigned long) opt->val;
2299                 break;
2300         }
2301
2302         BT_DBG("type 0x%2.2x len %d val 0x%lx", *type, opt->len, *val);
2303         return len;
2304 }
2305
2306 static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val)
2307 {
2308         struct l2cap_conf_opt *opt = *ptr;
2309
2310         BT_DBG("type 0x%2.2x len %d val 0x%lx", type, len, val);
2311
2312         opt->type = type;
2313         opt->len  = len;
2314
2315         switch (len) {
2316         case 1:
2317                 *((u8 *) opt->val)  = val;
2318                 break;
2319
2320         case 2:
2321                 *((__le16 *) opt->val) = cpu_to_le16(val);
2322                 break;
2323
2324         case 4:
2325                 *((__le32 *) opt->val) = cpu_to_le32(val);
2326                 break;
2327
2328         default:
2329                 memcpy(opt->val, (void *) val, len);
2330                 break;
2331         }
2332
2333         *ptr += L2CAP_CONF_OPT_SIZE + len;
2334 }
2335
2336 static void l2cap_ack_timeout(unsigned long arg)
2337 {
2338         struct sock *sk = (void *) arg;
2339
2340         bh_lock_sock(sk);
2341         l2cap_send_ack(l2cap_pi(sk));
2342         bh_unlock_sock(sk);
2343 }
2344
2345 static inline void l2cap_ertm_init(struct sock *sk)
2346 {
2347         l2cap_pi(sk)->expected_ack_seq = 0;
2348         l2cap_pi(sk)->unacked_frames = 0;
2349         l2cap_pi(sk)->buffer_seq = 0;
2350         l2cap_pi(sk)->num_acked = 0;
2351         l2cap_pi(sk)->frames_sent = 0;
2352
2353         setup_timer(&l2cap_pi(sk)->retrans_timer,
2354                         l2cap_retrans_timeout, (unsigned long) sk);
2355         setup_timer(&l2cap_pi(sk)->monitor_timer,
2356                         l2cap_monitor_timeout, (unsigned long) sk);
2357         setup_timer(&l2cap_pi(sk)->ack_timer,
2358                         l2cap_ack_timeout, (unsigned long) sk);
2359
2360         __skb_queue_head_init(SREJ_QUEUE(sk));
2361         __skb_queue_head_init(BUSY_QUEUE(sk));
2362         spin_lock_init(&l2cap_pi(sk)->send_lock);
2363
2364         INIT_WORK(&l2cap_pi(sk)->busy_work, l2cap_busy_work);
2365 }
2366
2367 static int l2cap_mode_supported(__u8 mode, __u32 feat_mask)
2368 {
2369         u32 local_feat_mask = l2cap_feat_mask;
2370         if (enable_ertm)
2371                 local_feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING;
2372
2373         switch (mode) {
2374         case L2CAP_MODE_ERTM:
2375                 return L2CAP_FEAT_ERTM & feat_mask & local_feat_mask;
2376         case L2CAP_MODE_STREAMING:
2377                 return L2CAP_FEAT_STREAMING & feat_mask & local_feat_mask;
2378         default:
2379                 return 0x00;
2380         }
2381 }
2382
2383 static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask)
2384 {
2385         switch (mode) {
2386         case L2CAP_MODE_STREAMING:
2387         case L2CAP_MODE_ERTM:
2388                 if (l2cap_mode_supported(mode, remote_feat_mask))
2389                         return mode;
2390                 /* fall through */
2391         default:
2392                 return L2CAP_MODE_BASIC;
2393         }
2394 }
2395
2396 static int l2cap_build_conf_req(struct sock *sk, void *data)
2397 {
2398         struct l2cap_pinfo *pi = l2cap_pi(sk);
2399         struct l2cap_conf_req *req = data;
2400         struct l2cap_conf_rfc rfc = { .mode = pi->mode };
2401         void *ptr = req->data;
2402
2403         BT_DBG("sk %p", sk);
2404
2405         if (pi->num_conf_req || pi->num_conf_rsp)
2406                 goto done;
2407
2408         switch (pi->mode) {
2409         case L2CAP_MODE_STREAMING:
2410         case L2CAP_MODE_ERTM:
2411                 pi->conf_state |= L2CAP_CONF_STATE2_DEVICE;
2412                 if (!l2cap_mode_supported(pi->mode, pi->conn->feat_mask))
2413                         l2cap_send_disconn_req(pi->conn, sk);
2414                 break;
2415         default:
2416                 pi->mode = l2cap_select_mode(rfc.mode, pi->conn->feat_mask);
2417                 break;
2418         }
2419
2420 done:
2421         switch (pi->mode) {
2422         case L2CAP_MODE_BASIC:
2423                 if (pi->imtu != L2CAP_DEFAULT_MTU)
2424                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->imtu);
2425                 break;
2426
2427         case L2CAP_MODE_ERTM:
2428                 rfc.mode            = L2CAP_MODE_ERTM;
2429                 rfc.txwin_size      = pi->tx_win;
2430                 rfc.max_transmit    = pi->max_tx;
2431                 rfc.retrans_timeout = 0;
2432                 rfc.monitor_timeout = 0;
2433                 rfc.max_pdu_size    = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE);
2434                 if (L2CAP_DEFAULT_MAX_PDU_SIZE > pi->conn->mtu - 10)
2435                         rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10);
2436
2437                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2438                                         sizeof(rfc), (unsigned long) &rfc);
2439
2440                 if (!(pi->conn->feat_mask & L2CAP_FEAT_FCS))
2441                         break;
2442
2443                 if (pi->fcs == L2CAP_FCS_NONE ||
2444                                 pi->conf_state & L2CAP_CONF_NO_FCS_RECV) {
2445                         pi->fcs = L2CAP_FCS_NONE;
2446                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, pi->fcs);
2447                 }
2448                 break;
2449
2450         case L2CAP_MODE_STREAMING:
2451                 rfc.mode            = L2CAP_MODE_STREAMING;
2452                 rfc.txwin_size      = 0;
2453                 rfc.max_transmit    = 0;
2454                 rfc.retrans_timeout = 0;
2455                 rfc.monitor_timeout = 0;
2456                 rfc.max_pdu_size    = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE);
2457                 if (L2CAP_DEFAULT_MAX_PDU_SIZE > pi->conn->mtu - 10)
2458                         rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10);
2459
2460                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2461                                         sizeof(rfc), (unsigned long) &rfc);
2462
2463                 if (!(pi->conn->feat_mask & L2CAP_FEAT_FCS))
2464                         break;
2465
2466                 if (pi->fcs == L2CAP_FCS_NONE ||
2467                                 pi->conf_state & L2CAP_CONF_NO_FCS_RECV) {
2468                         pi->fcs = L2CAP_FCS_NONE;
2469                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, pi->fcs);
2470                 }
2471                 break;
2472         }
2473
2474         /* FIXME: Need actual value of the flush timeout */
2475         //if (flush_to != L2CAP_DEFAULT_FLUSH_TO)
2476         //   l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO, 2, pi->flush_to);
2477
2478         req->dcid  = cpu_to_le16(pi->dcid);
2479         req->flags = cpu_to_le16(0);
2480
2481         return ptr - data;
2482 }
2483
2484 static int l2cap_parse_conf_req(struct sock *sk, void *data)
2485 {
2486         struct l2cap_pinfo *pi = l2cap_pi(sk);
2487         struct l2cap_conf_rsp *rsp = data;
2488         void *ptr = rsp->data;
2489         void *req = pi->conf_req;
2490         int len = pi->conf_len;
2491         int type, hint, olen;
2492         unsigned long val;
2493         struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
2494         u16 mtu = L2CAP_DEFAULT_MTU;
2495         u16 result = L2CAP_CONF_SUCCESS;
2496
2497         BT_DBG("sk %p", sk);
2498
2499         while (len >= L2CAP_CONF_OPT_SIZE) {
2500                 len -= l2cap_get_conf_opt(&req, &type, &olen, &val);
2501
2502                 hint  = type & L2CAP_CONF_HINT;
2503                 type &= L2CAP_CONF_MASK;
2504
2505                 switch (type) {
2506                 case L2CAP_CONF_MTU:
2507                         mtu = val;
2508                         break;
2509
2510                 case L2CAP_CONF_FLUSH_TO:
2511                         pi->flush_to = val;
2512                         break;
2513
2514                 case L2CAP_CONF_QOS:
2515                         break;
2516
2517                 case L2CAP_CONF_RFC:
2518                         if (olen == sizeof(rfc))
2519                                 memcpy(&rfc, (void *) val, olen);
2520                         break;
2521
2522                 case L2CAP_CONF_FCS:
2523                         if (val == L2CAP_FCS_NONE)
2524                                 pi->conf_state |= L2CAP_CONF_NO_FCS_RECV;
2525
2526                         break;
2527
2528                 default:
2529                         if (hint)
2530                                 break;
2531
2532                         result = L2CAP_CONF_UNKNOWN;
2533                         *((u8 *) ptr++) = type;
2534                         break;
2535                 }
2536         }
2537
2538         if (pi->num_conf_rsp || pi->num_conf_req)
2539                 goto done;
2540
2541         switch (pi->mode) {
2542         case L2CAP_MODE_STREAMING:
2543         case L2CAP_MODE_ERTM:
2544                 pi->conf_state |= L2CAP_CONF_STATE2_DEVICE;
2545                 if (!l2cap_mode_supported(pi->mode, pi->conn->feat_mask))
2546                         return -ECONNREFUSED;
2547                 break;
2548         default:
2549                 pi->mode = l2cap_select_mode(rfc.mode, pi->conn->feat_mask);
2550                 break;
2551         }
2552
2553 done:
2554         if (pi->mode != rfc.mode) {
2555                 result = L2CAP_CONF_UNACCEPT;
2556                 rfc.mode = pi->mode;
2557
2558                 if (pi->num_conf_rsp == 1)
2559                         return -ECONNREFUSED;
2560
2561                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2562                                         sizeof(rfc), (unsigned long) &rfc);
2563         }
2564
2565
2566         if (result == L2CAP_CONF_SUCCESS) {
2567                 /* Configure output options and let the other side know
2568                  * which ones we don't like. */
2569
2570                 if (mtu < L2CAP_DEFAULT_MIN_MTU)
2571                         result = L2CAP_CONF_UNACCEPT;
2572                 else {
2573                         pi->omtu = mtu;
2574                         pi->conf_state |= L2CAP_CONF_MTU_DONE;
2575                 }
2576                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->omtu);
2577
2578                 switch (rfc.mode) {
2579                 case L2CAP_MODE_BASIC:
2580                         pi->fcs = L2CAP_FCS_NONE;
2581                         pi->conf_state |= L2CAP_CONF_MODE_DONE;
2582                         break;
2583
2584                 case L2CAP_MODE_ERTM:
2585                         pi->remote_tx_win = rfc.txwin_size;
2586                         pi->remote_max_tx = rfc.max_transmit;
2587                         if (rfc.max_pdu_size > pi->conn->mtu - 10)
2588                                 rfc.max_pdu_size = le16_to_cpu(pi->conn->mtu - 10);
2589
2590                         pi->remote_mps = le16_to_cpu(rfc.max_pdu_size);
2591
2592                         rfc.retrans_timeout =
2593                                 le16_to_cpu(L2CAP_DEFAULT_RETRANS_TO);
2594                         rfc.monitor_timeout =
2595                                 le16_to_cpu(L2CAP_DEFAULT_MONITOR_TO);
2596
2597                         pi->conf_state |= L2CAP_CONF_MODE_DONE;
2598
2599                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2600                                         sizeof(rfc), (unsigned long) &rfc);
2601
2602                         break;
2603
2604                 case L2CAP_MODE_STREAMING:
2605                         if (rfc.max_pdu_size > pi->conn->mtu - 10)
2606                                 rfc.max_pdu_size = le16_to_cpu(pi->conn->mtu - 10);
2607
2608                         pi->remote_mps = le16_to_cpu(rfc.max_pdu_size);
2609
2610                         pi->conf_state |= L2CAP_CONF_MODE_DONE;
2611
2612                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2613                                         sizeof(rfc), (unsigned long) &rfc);
2614
2615                         break;
2616
2617                 default:
2618                         result = L2CAP_CONF_UNACCEPT;
2619
2620                         memset(&rfc, 0, sizeof(rfc));
2621                         rfc.mode = pi->mode;
2622                 }
2623
2624                 if (result == L2CAP_CONF_SUCCESS)
2625                         pi->conf_state |= L2CAP_CONF_OUTPUT_DONE;
2626         }
2627         rsp->scid   = cpu_to_le16(pi->dcid);
2628         rsp->result = cpu_to_le16(result);
2629         rsp->flags  = cpu_to_le16(0x0000);
2630
2631         return ptr - data;
2632 }
2633
2634 static int l2cap_parse_conf_rsp(struct sock *sk, void *rsp, int len, void *data, u16 *result)
2635 {
2636         struct l2cap_pinfo *pi = l2cap_pi(sk);
2637         struct l2cap_conf_req *req = data;
2638         void *ptr = req->data;
2639         int type, olen;
2640         unsigned long val;
2641         struct l2cap_conf_rfc rfc;
2642
2643         BT_DBG("sk %p, rsp %p, len %d, req %p", sk, rsp, len, data);
2644
2645         while (len >= L2CAP_CONF_OPT_SIZE) {
2646                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
2647
2648                 switch (type) {
2649                 case L2CAP_CONF_MTU:
2650                         if (val < L2CAP_DEFAULT_MIN_MTU) {
2651                                 *result = L2CAP_CONF_UNACCEPT;
2652                                 pi->omtu = L2CAP_DEFAULT_MIN_MTU;
2653                         } else
2654                                 pi->omtu = val;
2655                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->omtu);
2656                         break;
2657
2658                 case L2CAP_CONF_FLUSH_TO:
2659                         pi->flush_to = val;
2660                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
2661                                                         2, pi->flush_to);
2662                         break;
2663
2664                 case L2CAP_CONF_RFC:
2665                         if (olen == sizeof(rfc))
2666                                 memcpy(&rfc, (void *)val, olen);
2667
2668                         if ((pi->conf_state & L2CAP_CONF_STATE2_DEVICE) &&
2669                                                         rfc.mode != pi->mode)
2670                                 return -ECONNREFUSED;
2671
2672                         pi->mode = rfc.mode;
2673                         pi->fcs = 0;
2674
2675                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2676                                         sizeof(rfc), (unsigned long) &rfc);
2677                         break;
2678                 }
2679         }
2680
2681         if (*result == L2CAP_CONF_SUCCESS) {
2682                 switch (rfc.mode) {
2683                 case L2CAP_MODE_ERTM:
2684                         pi->remote_tx_win   = rfc.txwin_size;
2685                         pi->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
2686                         pi->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
2687                         pi->mps    = le16_to_cpu(rfc.max_pdu_size);
2688                         break;
2689                 case L2CAP_MODE_STREAMING:
2690                         pi->mps    = le16_to_cpu(rfc.max_pdu_size);
2691                 }
2692         }
2693
2694         req->dcid   = cpu_to_le16(pi->dcid);
2695         req->flags  = cpu_to_le16(0x0000);
2696
2697         return ptr - data;
2698 }
2699
2700 static int l2cap_build_conf_rsp(struct sock *sk, void *data, u16 result, u16 flags)
2701 {
2702         struct l2cap_conf_rsp *rsp = data;
2703         void *ptr = rsp->data;
2704
2705         BT_DBG("sk %p", sk);
2706
2707         rsp->scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
2708         rsp->result = cpu_to_le16(result);
2709         rsp->flags  = cpu_to_le16(flags);
2710
2711         return ptr - data;
2712 }
2713
2714 static void l2cap_conf_rfc_get(struct sock *sk, void *rsp, int len)
2715 {
2716         struct l2cap_pinfo *pi = l2cap_pi(sk);
2717         int type, olen;
2718         unsigned long val;
2719         struct l2cap_conf_rfc rfc;
2720
2721         BT_DBG("sk %p, rsp %p, len %d", sk, rsp, len);
2722
2723         if ((pi->mode != L2CAP_MODE_ERTM) && (pi->mode != L2CAP_MODE_STREAMING))
2724                 return;
2725
2726         while (len >= L2CAP_CONF_OPT_SIZE) {
2727                 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
2728
2729                 switch (type) {
2730                 case L2CAP_CONF_RFC:
2731                         if (olen == sizeof(rfc))
2732                                 memcpy(&rfc, (void *)val, olen);
2733                         goto done;
2734                 }
2735         }
2736
2737 done:
2738         switch (rfc.mode) {
2739         case L2CAP_MODE_ERTM:
2740                 pi->remote_tx_win   = rfc.txwin_size;
2741                 pi->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
2742                 pi->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
2743                 pi->mps    = le16_to_cpu(rfc.max_pdu_size);
2744                 break;
2745         case L2CAP_MODE_STREAMING:
2746                 pi->mps    = le16_to_cpu(rfc.max_pdu_size);
2747         }
2748 }
2749
2750 static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2751 {
2752         struct l2cap_cmd_rej *rej = (struct l2cap_cmd_rej *) data;
2753
2754         if (rej->reason != 0x0000)
2755                 return 0;
2756
2757         if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
2758                                         cmd->ident == conn->info_ident) {
2759                 del_timer(&conn->info_timer);
2760
2761                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2762                 conn->info_ident = 0;
2763
2764                 l2cap_conn_start(conn);
2765         }
2766
2767         return 0;
2768 }
2769
2770 static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2771 {
2772         struct l2cap_chan_list *list = &conn->chan_list;
2773         struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
2774         struct l2cap_conn_rsp rsp;
2775         struct sock *sk, *parent;
2776         int result, status = L2CAP_CS_NO_INFO;
2777
2778         u16 dcid = 0, scid = __le16_to_cpu(req->scid);
2779         __le16 psm = req->psm;
2780
2781         BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid);
2782
2783         /* Check if we have socket listening on psm */
2784         parent = l2cap_get_sock_by_psm(BT_LISTEN, psm, conn->src);
2785         if (!parent) {
2786                 result = L2CAP_CR_BAD_PSM;
2787                 goto sendresp;
2788         }
2789
2790         /* Check if the ACL is secure enough (if not SDP) */
2791         if (psm != cpu_to_le16(0x0001) &&
2792                                 !hci_conn_check_link_mode(conn->hcon)) {
2793                 conn->disc_reason = 0x05;
2794                 result = L2CAP_CR_SEC_BLOCK;
2795                 goto response;
2796         }
2797
2798         result = L2CAP_CR_NO_MEM;
2799
2800         /* Check for backlog size */
2801         if (sk_acceptq_is_full(parent)) {
2802                 BT_DBG("backlog full %d", parent->sk_ack_backlog);
2803                 goto response;
2804         }
2805
2806         sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP, GFP_ATOMIC);
2807         if (!sk)
2808                 goto response;
2809
2810         write_lock_bh(&list->lock);
2811
2812         /* Check if we already have channel with that dcid */
2813         if (__l2cap_get_chan_by_dcid(list, scid)) {
2814                 write_unlock_bh(&list->lock);
2815                 sock_set_flag(sk, SOCK_ZAPPED);
2816                 l2cap_sock_kill(sk);
2817                 goto response;
2818         }
2819
2820         hci_conn_hold(conn->hcon);
2821
2822         l2cap_sock_init(sk, parent);
2823         bacpy(&bt_sk(sk)->src, conn->src);
2824         bacpy(&bt_sk(sk)->dst, conn->dst);
2825         l2cap_pi(sk)->psm  = psm;
2826         l2cap_pi(sk)->dcid = scid;
2827
2828         __l2cap_chan_add(conn, sk, parent);
2829         dcid = l2cap_pi(sk)->scid;
2830
2831         l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
2832
2833         l2cap_pi(sk)->ident = cmd->ident;
2834
2835         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
2836                 if (l2cap_check_security(sk)) {
2837                         if (bt_sk(sk)->defer_setup) {
2838                                 sk->sk_state = BT_CONNECT2;
2839                                 result = L2CAP_CR_PEND;
2840                                 status = L2CAP_CS_AUTHOR_PEND;
2841                                 parent->sk_data_ready(parent, 0);
2842                         } else {
2843                                 sk->sk_state = BT_CONFIG;
2844                                 result = L2CAP_CR_SUCCESS;
2845                                 status = L2CAP_CS_NO_INFO;
2846                         }
2847                 } else {
2848                         sk->sk_state = BT_CONNECT2;
2849                         result = L2CAP_CR_PEND;
2850                         status = L2CAP_CS_AUTHEN_PEND;
2851                 }
2852         } else {
2853                 sk->sk_state = BT_CONNECT2;
2854                 result = L2CAP_CR_PEND;
2855                 status = L2CAP_CS_NO_INFO;
2856         }
2857
2858         write_unlock_bh(&list->lock);
2859
2860 response:
2861         bh_unlock_sock(parent);
2862
2863 sendresp:
2864         rsp.scid   = cpu_to_le16(scid);
2865         rsp.dcid   = cpu_to_le16(dcid);
2866         rsp.result = cpu_to_le16(result);
2867         rsp.status = cpu_to_le16(status);
2868         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
2869
2870         if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
2871                 struct l2cap_info_req info;
2872                 info.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
2873
2874                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
2875                 conn->info_ident = l2cap_get_ident(conn);
2876
2877                 mod_timer(&conn->info_timer, jiffies +
2878                                         msecs_to_jiffies(L2CAP_INFO_TIMEOUT));
2879
2880                 l2cap_send_cmd(conn, conn->info_ident,
2881                                         L2CAP_INFO_REQ, sizeof(info), &info);
2882         }
2883
2884         return 0;
2885 }
2886
2887 static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2888 {
2889         struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
2890         u16 scid, dcid, result, status;
2891         struct sock *sk;
2892         u8 req[128];
2893
2894         scid   = __le16_to_cpu(rsp->scid);
2895         dcid   = __le16_to_cpu(rsp->dcid);
2896         result = __le16_to_cpu(rsp->result);
2897         status = __le16_to_cpu(rsp->status);
2898
2899         BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", dcid, scid, result, status);
2900
2901         if (scid) {
2902                 sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
2903                 if (!sk)
2904                         return 0;
2905         } else {
2906                 sk = l2cap_get_chan_by_ident(&conn->chan_list, cmd->ident);
2907                 if (!sk)
2908                         return 0;
2909         }
2910
2911         switch (result) {
2912         case L2CAP_CR_SUCCESS:
2913                 sk->sk_state = BT_CONFIG;
2914                 l2cap_pi(sk)->ident = 0;
2915                 l2cap_pi(sk)->dcid = dcid;
2916                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
2917
2918                 l2cap_pi(sk)->conf_state &= ~L2CAP_CONF_CONNECT_PEND;
2919
2920                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2921                                         l2cap_build_conf_req(sk, req), req);
2922                 l2cap_pi(sk)->num_conf_req++;
2923                 break;
2924
2925         case L2CAP_CR_PEND:
2926                 l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND;
2927                 break;
2928
2929         default:
2930                 l2cap_chan_del(sk, ECONNREFUSED);
2931                 break;
2932         }
2933
2934         bh_unlock_sock(sk);
2935         return 0;
2936 }
2937
2938 static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
2939 {
2940         struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
2941         u16 dcid, flags;
2942         u8 rsp[64];
2943         struct sock *sk;
2944         int len;
2945
2946         dcid  = __le16_to_cpu(req->dcid);
2947         flags = __le16_to_cpu(req->flags);
2948
2949         BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
2950
2951         sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid);
2952         if (!sk)
2953                 return -ENOENT;
2954
2955         if (sk->sk_state == BT_DISCONN)
2956                 goto unlock;
2957
2958         /* Reject if config buffer is too small. */
2959         len = cmd_len - sizeof(*req);
2960         if (l2cap_pi(sk)->conf_len + len > sizeof(l2cap_pi(sk)->conf_req)) {
2961                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2962                                 l2cap_build_conf_rsp(sk, rsp,
2963                                         L2CAP_CONF_REJECT, flags), rsp);
2964                 goto unlock;
2965         }
2966
2967         /* Store config. */
2968         memcpy(l2cap_pi(sk)->conf_req + l2cap_pi(sk)->conf_len, req->data, len);
2969         l2cap_pi(sk)->conf_len += len;
2970
2971         if (flags & 0x0001) {
2972                 /* Incomplete config. Send empty response. */
2973                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2974                                 l2cap_build_conf_rsp(sk, rsp,
2975                                         L2CAP_CONF_SUCCESS, 0x0001), rsp);
2976                 goto unlock;
2977         }
2978
2979         /* Complete config. */
2980         len = l2cap_parse_conf_req(sk, rsp);
2981         if (len < 0) {
2982                 l2cap_send_disconn_req(conn, sk);
2983                 goto unlock;
2984         }
2985
2986         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
2987         l2cap_pi(sk)->num_conf_rsp++;
2988
2989         /* Reset config buffer. */
2990         l2cap_pi(sk)->conf_len = 0;
2991
2992         if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE))
2993                 goto unlock;
2994
2995         if (l2cap_pi(sk)->conf_state & L2CAP_CONF_INPUT_DONE) {
2996                 if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_NO_FCS_RECV) ||
2997                     l2cap_pi(sk)->fcs != L2CAP_FCS_NONE)
2998                         l2cap_pi(sk)->fcs = L2CAP_FCS_CRC16;
2999
3000                 sk->sk_state = BT_CONNECTED;
3001
3002                 l2cap_pi(sk)->next_tx_seq = 0;
3003                 l2cap_pi(sk)->expected_tx_seq = 0;
3004                 __skb_queue_head_init(TX_QUEUE(sk));
3005                 if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM)
3006                         l2cap_ertm_init(sk);
3007
3008                 l2cap_chan_ready(sk);
3009                 goto unlock;
3010         }
3011
3012         if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT)) {
3013                 u8 buf[64];
3014                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3015                                         l2cap_build_conf_req(sk, buf), buf);
3016                 l2cap_pi(sk)->num_conf_req++;
3017         }
3018
3019 unlock:
3020         bh_unlock_sock(sk);
3021         return 0;
3022 }
3023
3024 static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3025 {
3026         struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
3027         u16 scid, flags, result;
3028         struct sock *sk;
3029         int len = cmd->len - sizeof(*rsp);
3030
3031         scid   = __le16_to_cpu(rsp->scid);
3032         flags  = __le16_to_cpu(rsp->flags);
3033         result = __le16_to_cpu(rsp->result);
3034
3035         BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x",
3036                         scid, flags, result);
3037
3038         sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
3039         if (!sk)
3040                 return 0;
3041
3042         switch (result) {
3043         case L2CAP_CONF_SUCCESS:
3044                 l2cap_conf_rfc_get(sk, rsp->data, len);
3045                 break;
3046
3047         case L2CAP_CONF_UNACCEPT:
3048                 if (l2cap_pi(sk)->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
3049                         char req[64];
3050
3051                         if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) {
3052                                 l2cap_send_disconn_req(conn, sk);
3053                                 goto done;
3054                         }
3055
3056                         /* throw out any old stored conf requests */
3057                         result = L2CAP_CONF_SUCCESS;
3058                         len = l2cap_parse_conf_rsp(sk, rsp->data,
3059                                                         len, req, &result);
3060                         if (len < 0) {
3061                                 l2cap_send_disconn_req(conn, sk);
3062                                 goto done;
3063                         }
3064
3065                         l2cap_send_cmd(conn, l2cap_get_ident(conn),
3066                                                 L2CAP_CONF_REQ, len, req);
3067                         l2cap_pi(sk)->num_conf_req++;
3068                         if (result != L2CAP_CONF_SUCCESS)
3069                                 goto done;
3070                         break;
3071                 }
3072
3073         default:
3074                 sk->sk_state = BT_DISCONN;
3075                 sk->sk_err = ECONNRESET;
3076                 l2cap_sock_set_timer(sk, HZ * 5);
3077                 l2cap_send_disconn_req(conn, sk);
3078                 goto done;
3079         }
3080
3081         if (flags & 0x01)
3082                 goto done;
3083
3084         l2cap_pi(sk)->conf_state |= L2CAP_CONF_INPUT_DONE;
3085
3086         if (l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE) {
3087                 if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_NO_FCS_RECV) ||
3088                     l2cap_pi(sk)->fcs != L2CAP_FCS_NONE)
3089                         l2cap_pi(sk)->fcs = L2CAP_FCS_CRC16;
3090
3091                 sk->sk_state = BT_CONNECTED;
3092                 l2cap_pi(sk)->next_tx_seq = 0;
3093                 l2cap_pi(sk)->expected_tx_seq = 0;
3094                 __skb_queue_head_init(TX_QUEUE(sk));
3095                 if (l2cap_pi(sk)->mode ==  L2CAP_MODE_ERTM)
3096                         l2cap_ertm_init(sk);
3097
3098                 l2cap_chan_ready(sk);
3099         }
3100
3101 done:
3102         bh_unlock_sock(sk);
3103         return 0;
3104 }
3105
3106 static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3107 {
3108         struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
3109         struct l2cap_disconn_rsp rsp;
3110         u16 dcid, scid;
3111         struct sock *sk;
3112
3113         scid = __le16_to_cpu(req->scid);
3114         dcid = __le16_to_cpu(req->dcid);
3115
3116         BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
3117
3118         sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid);
3119         if (!sk)
3120                 return 0;
3121
3122         rsp.dcid = cpu_to_le16(l2cap_pi(sk)->scid);
3123         rsp.scid = cpu_to_le16(l2cap_pi(sk)->dcid);
3124         l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
3125
3126         sk->sk_shutdown = SHUTDOWN_MASK;
3127
3128         skb_queue_purge(TX_QUEUE(sk));
3129
3130         if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM) {
3131                 skb_queue_purge(SREJ_QUEUE(sk));
3132                 skb_queue_purge(BUSY_QUEUE(sk));
3133                 del_timer(&l2cap_pi(sk)->retrans_timer);
3134                 del_timer(&l2cap_pi(sk)->monitor_timer);
3135                 del_timer(&l2cap_pi(sk)->ack_timer);
3136         }
3137
3138         l2cap_chan_del(sk, ECONNRESET);
3139         bh_unlock_sock(sk);
3140
3141         l2cap_sock_kill(sk);
3142         return 0;
3143 }
3144
3145 static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3146 {
3147         struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
3148         u16 dcid, scid;
3149         struct sock *sk;
3150
3151         scid = __le16_to_cpu(rsp->scid);
3152         dcid = __le16_to_cpu(rsp->dcid);
3153
3154         BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
3155
3156         sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
3157         if (!sk)
3158                 return 0;
3159
3160         skb_queue_purge(TX_QUEUE(sk));
3161
3162         if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM) {
3163                 skb_queue_purge(SREJ_QUEUE(sk));
3164                 skb_queue_purge(BUSY_QUEUE(sk));
3165                 del_timer(&l2cap_pi(sk)->retrans_timer);
3166                 del_timer(&l2cap_pi(sk)->monitor_timer);
3167                 del_timer(&l2cap_pi(sk)->ack_timer);
3168         }
3169
3170         l2cap_chan_del(sk, 0);
3171         bh_unlock_sock(sk);
3172
3173         l2cap_sock_kill(sk);
3174         return 0;
3175 }
3176
3177 static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3178 {
3179         struct l2cap_info_req *req = (struct l2cap_info_req *) data;
3180         u16 type;
3181
3182         type = __le16_to_cpu(req->type);
3183
3184         BT_DBG("type 0x%4.4x", type);
3185
3186         if (type == L2CAP_IT_FEAT_MASK) {
3187                 u8 buf[8];
3188                 u32 feat_mask = l2cap_feat_mask;
3189                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
3190                 rsp->type   = cpu_to_le16(L2CAP_IT_FEAT_MASK);
3191                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
3192                 if (enable_ertm)
3193                         feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
3194                                                          | L2CAP_FEAT_FCS;
3195                 put_unaligned_le32(feat_mask, rsp->data);
3196                 l2cap_send_cmd(conn, cmd->ident,
3197                                         L2CAP_INFO_RSP, sizeof(buf), buf);
3198         } else if (type == L2CAP_IT_FIXED_CHAN) {
3199                 u8 buf[12];
3200                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
3201                 rsp->type   = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
3202                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
3203                 memcpy(buf + 4, l2cap_fixed_chan, 8);
3204                 l2cap_send_cmd(conn, cmd->ident,
3205                                         L2CAP_INFO_RSP, sizeof(buf), buf);
3206         } else {
3207                 struct l2cap_info_rsp rsp;
3208                 rsp.type   = cpu_to_le16(type);
3209                 rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP);
3210                 l2cap_send_cmd(conn, cmd->ident,
3211                                         L2CAP_INFO_RSP, sizeof(rsp), &rsp);
3212         }
3213
3214         return 0;
3215 }
3216
3217 static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
3218 {
3219         struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
3220         u16 type, result;
3221
3222         type   = __le16_to_cpu(rsp->type);
3223         result = __le16_to_cpu(rsp->result);
3224
3225         BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
3226
3227         del_timer(&conn->info_timer);
3228
3229         if (type == L2CAP_IT_FEAT_MASK) {
3230                 conn->feat_mask = get_unaligned_le32(rsp->data);
3231
3232                 if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
3233                         struct l2cap_info_req req;
3234                         req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
3235
3236                         conn->info_ident = l2cap_get_ident(conn);
3237
3238                         l2cap_send_cmd(conn, conn->info_ident,
3239                                         L2CAP_INFO_REQ, sizeof(req), &req);
3240                 } else {
3241                         conn->info_state