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