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