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