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