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