projects / GitHub / mt8127 / android_kernel_alcatel_ttab.git / blob
? search:


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