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