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