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