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