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