1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31#include <linux/module.h>
32
33#include <linux/debugfs.h>
34#include <linux/crc16.h>
35
36#include <net/bluetooth/bluetooth.h>
37#include <net/bluetooth/hci_core.h>
38#include <net/bluetooth/l2cap.h>
39#include <net/bluetooth/smp.h>
40#include <net/bluetooth/a2mp.h>
41#include <net/bluetooth/amp.h>
42
43bool disable_ertm;
44
45static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN;
46static u8 l2cap_fixed_chan[8] = { L2CAP_FC_L2CAP, };
47
48static LIST_HEAD(chan_list);
49static DEFINE_RWLOCK(chan_list_lock);
50
51static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
52 u8 code, u8 ident, u16 dlen, void *data);
53static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len,
54 void *data);
55static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data);
56static void l2cap_send_disconn_req(struct l2cap_chan *chan, int err);
57
58static void l2cap_tx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
59 struct sk_buff_head *skbs, u8 event);
60
61
62
63static struct l2cap_chan *__l2cap_get_chan_by_dcid(struct l2cap_conn *conn,
64 u16 cid)
65{
66 struct l2cap_chan *c;
67
68 list_for_each_entry(c, &conn->chan_l, list) {
69 if (c->dcid == cid)
70 return c;
71 }
72 return NULL;
73}
74
75static struct l2cap_chan *__l2cap_get_chan_by_scid(struct l2cap_conn *conn,
76 u16 cid)
77{
78 struct l2cap_chan *c;
79
80 list_for_each_entry(c, &conn->chan_l, list) {
81 if (c->scid == cid)
82 return c;
83 }
84 return NULL;
85}
86
87
88
89static struct l2cap_chan *l2cap_get_chan_by_scid(struct l2cap_conn *conn,
90 u16 cid)
91{
92 struct l2cap_chan *c;
93
94 mutex_lock(&conn->chan_lock);
95 c = __l2cap_get_chan_by_scid(conn, cid);
96 if (c)
97 l2cap_chan_lock(c);
98 mutex_unlock(&conn->chan_lock);
99
100 return c;
101}
102
103
104
105
106static struct l2cap_chan *l2cap_get_chan_by_dcid(struct l2cap_conn *conn,
107 u16 cid)
108{
109 struct l2cap_chan *c;
110
111 mutex_lock(&conn->chan_lock);
112 c = __l2cap_get_chan_by_dcid(conn, cid);
113 if (c)
114 l2cap_chan_lock(c);
115 mutex_unlock(&conn->chan_lock);
116
117 return c;
118}
119
120static struct l2cap_chan *__l2cap_get_chan_by_ident(struct l2cap_conn *conn,
121 u8 ident)
122{
123 struct l2cap_chan *c;
124
125 list_for_each_entry(c, &conn->chan_l, list) {
126 if (c->ident == ident)
127 return c;
128 }
129 return NULL;
130}
131
132static struct l2cap_chan *l2cap_get_chan_by_ident(struct l2cap_conn *conn,
133 u8 ident)
134{
135 struct l2cap_chan *c;
136
137 mutex_lock(&conn->chan_lock);
138 c = __l2cap_get_chan_by_ident(conn, ident);
139 if (c)
140 l2cap_chan_lock(c);
141 mutex_unlock(&conn->chan_lock);
142
143 return c;
144}
145
146static struct l2cap_chan *__l2cap_global_chan_by_addr(__le16 psm, bdaddr_t *src)
147{
148 struct l2cap_chan *c;
149
150 list_for_each_entry(c, &chan_list, global_l) {
151 if (c->sport == psm && !bacmp(&bt_sk(c->sk)->src, src))
152 return c;
153 }
154 return NULL;
155}
156
157int l2cap_add_psm(struct l2cap_chan *chan, bdaddr_t *src, __le16 psm)
158{
159 int err;
160
161 write_lock(&chan_list_lock);
162
163 if (psm && __l2cap_global_chan_by_addr(psm, src)) {
164 err = -EADDRINUSE;
165 goto done;
166 }
167
168 if (psm) {
169 chan->psm = psm;
170 chan->sport = psm;
171 err = 0;
172 } else {
173 u16 p;
174
175 err = -EINVAL;
176 for (p = 0x1001; p < 0x1100; p += 2)
177 if (!__l2cap_global_chan_by_addr(cpu_to_le16(p), src)) {
178 chan->psm = cpu_to_le16(p);
179 chan->sport = cpu_to_le16(p);
180 err = 0;
181 break;
182 }
183 }
184
185done:
186 write_unlock(&chan_list_lock);
187 return err;
188}
189
190int l2cap_add_scid(struct l2cap_chan *chan, __u16 scid)
191{
192 write_lock(&chan_list_lock);
193
194 chan->scid = scid;
195
196 write_unlock(&chan_list_lock);
197
198 return 0;
199}
200
201static u16 l2cap_alloc_cid(struct l2cap_conn *conn)
202{
203 u16 cid = L2CAP_CID_DYN_START;
204
205 for (; cid < L2CAP_CID_DYN_END; cid++) {
206 if (!__l2cap_get_chan_by_scid(conn, cid))
207 return cid;
208 }
209
210 return 0;
211}
212
213static void __l2cap_state_change(struct l2cap_chan *chan, int state)
214{
215 BT_DBG("chan %p %s -> %s", chan, state_to_string(chan->state),
216 state_to_string(state));
217
218 chan->state = state;
219 chan->ops->state_change(chan, state);
220}
221
222static void l2cap_state_change(struct l2cap_chan *chan, int state)
223{
224 struct sock *sk = chan->sk;
225
226 lock_sock(sk);
227 __l2cap_state_change(chan, state);
228 release_sock(sk);
229}
230
231static inline void __l2cap_chan_set_err(struct l2cap_chan *chan, int err)
232{
233 struct sock *sk = chan->sk;
234
235 sk->sk_err = err;
236}
237
238static inline void l2cap_chan_set_err(struct l2cap_chan *chan, int err)
239{
240 struct sock *sk = chan->sk;
241
242 lock_sock(sk);
243 __l2cap_chan_set_err(chan, err);
244 release_sock(sk);
245}
246
247static void __set_retrans_timer(struct l2cap_chan *chan)
248{
249 if (!delayed_work_pending(&chan->monitor_timer) &&
250 chan->retrans_timeout) {
251 l2cap_set_timer(chan, &chan->retrans_timer,
252 msecs_to_jiffies(chan->retrans_timeout));
253 }
254}
255
256static void __set_monitor_timer(struct l2cap_chan *chan)
257{
258 __clear_retrans_timer(chan);
259 if (chan->monitor_timeout) {
260 l2cap_set_timer(chan, &chan->monitor_timer,
261 msecs_to_jiffies(chan->monitor_timeout));
262 }
263}
264
265static struct sk_buff *l2cap_ertm_seq_in_queue(struct sk_buff_head *head,
266 u16 seq)
267{
268 struct sk_buff *skb;
269
270 skb_queue_walk(head, skb) {
271 if (bt_cb(skb)->control.txseq == seq)
272 return skb;
273 }
274
275 return NULL;
276}
277
278
279
280
281
282
283
284
285
286
287
288
289static int l2cap_seq_list_init(struct l2cap_seq_list *seq_list, u16 size)
290{
291 size_t alloc_size, i;
292
293
294
295
296
297 alloc_size = roundup_pow_of_two(size);
298
299 seq_list->list = kmalloc(sizeof(u16) * alloc_size, GFP_KERNEL);
300 if (!seq_list->list)
301 return -ENOMEM;
302
303 seq_list->mask = alloc_size - 1;
304 seq_list->head = L2CAP_SEQ_LIST_CLEAR;
305 seq_list->tail = L2CAP_SEQ_LIST_CLEAR;
306 for (i = 0; i < alloc_size; i++)
307 seq_list->list[i] = L2CAP_SEQ_LIST_CLEAR;
308
309 return 0;
310}
311
312static inline void l2cap_seq_list_free(struct l2cap_seq_list *seq_list)
313{
314 kfree(seq_list->list);
315}
316
317static inline bool l2cap_seq_list_contains(struct l2cap_seq_list *seq_list,
318 u16 seq)
319{
320
321 return seq_list->list[seq & seq_list->mask] != L2CAP_SEQ_LIST_CLEAR;
322}
323
324static u16 l2cap_seq_list_remove(struct l2cap_seq_list *seq_list, u16 seq)
325{
326 u16 mask = seq_list->mask;
327
328 if (seq_list->head == L2CAP_SEQ_LIST_CLEAR) {
329
330 return L2CAP_SEQ_LIST_CLEAR;
331 } else if (seq_list->head == seq) {
332
333 seq_list->head = seq_list->list[seq & mask];
334 seq_list->list[seq & mask] = L2CAP_SEQ_LIST_CLEAR;
335
336 if (seq_list->head == L2CAP_SEQ_LIST_TAIL) {
337 seq_list->head = L2CAP_SEQ_LIST_CLEAR;
338 seq_list->tail = L2CAP_SEQ_LIST_CLEAR;
339 }
340 } else {
341
342 u16 prev = seq_list->head;
343 while (seq_list->list[prev & mask] != seq) {
344 prev = seq_list->list[prev & mask];
345 if (prev == L2CAP_SEQ_LIST_TAIL)
346 return L2CAP_SEQ_LIST_CLEAR;
347 }
348
349
350 seq_list->list[prev & mask] = seq_list->list[seq & mask];
351 seq_list->list[seq & mask] = L2CAP_SEQ_LIST_CLEAR;
352 if (seq_list->tail == seq)
353 seq_list->tail = prev;
354 }
355 return seq;
356}
357
358static inline u16 l2cap_seq_list_pop(struct l2cap_seq_list *seq_list)
359{
360
361 return l2cap_seq_list_remove(seq_list, seq_list->head);
362}
363
364static void l2cap_seq_list_clear(struct l2cap_seq_list *seq_list)
365{
366 u16 i;
367
368 if (seq_list->head == L2CAP_SEQ_LIST_CLEAR)
369 return;
370
371 for (i = 0; i <= seq_list->mask; i++)
372 seq_list->list[i] = L2CAP_SEQ_LIST_CLEAR;
373
374 seq_list->head = L2CAP_SEQ_LIST_CLEAR;
375 seq_list->tail = L2CAP_SEQ_LIST_CLEAR;
376}
377
378static void l2cap_seq_list_append(struct l2cap_seq_list *seq_list, u16 seq)
379{
380 u16 mask = seq_list->mask;
381
382
383
384 if (seq_list->list[seq & mask] != L2CAP_SEQ_LIST_CLEAR)
385 return;
386
387 if (seq_list->tail == L2CAP_SEQ_LIST_CLEAR)
388 seq_list->head = seq;
389 else
390 seq_list->list[seq_list->tail & mask] = seq;
391
392 seq_list->tail = seq;
393 seq_list->list[seq & mask] = L2CAP_SEQ_LIST_TAIL;
394}
395
396static void l2cap_chan_timeout(struct work_struct *work)
397{
398 struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
399 chan_timer.work);
400 struct l2cap_conn *conn = chan->conn;
401 int reason;
402
403 BT_DBG("chan %p state %s", chan, state_to_string(chan->state));
404
405 mutex_lock(&conn->chan_lock);
406 l2cap_chan_lock(chan);
407
408 if (chan->state == BT_CONNECTED || chan->state == BT_CONFIG)
409 reason = ECONNREFUSED;
410 else if (chan->state == BT_CONNECT &&
411 chan->sec_level != BT_SECURITY_SDP)
412 reason = ECONNREFUSED;
413 else
414 reason = ETIMEDOUT;
415
416 l2cap_chan_close(chan, reason);
417
418 l2cap_chan_unlock(chan);
419
420 chan->ops->close(chan);
421 mutex_unlock(&conn->chan_lock);
422
423 l2cap_chan_put(chan);
424}
425
426struct l2cap_chan *l2cap_chan_create(void)
427{
428 struct l2cap_chan *chan;
429
430 chan = kzalloc(sizeof(*chan), GFP_ATOMIC);
431 if (!chan)
432 return NULL;
433
434 mutex_init(&chan->lock);
435
436 write_lock(&chan_list_lock);
437 list_add(&chan->global_l, &chan_list);
438 write_unlock(&chan_list_lock);
439
440 INIT_DELAYED_WORK(&chan->chan_timer, l2cap_chan_timeout);
441
442 chan->state = BT_OPEN;
443
444 kref_init(&chan->kref);
445
446
447 set_bit(CONF_NOT_COMPLETE, &chan->conf_state);
448
449 BT_DBG("chan %p", chan);
450
451 return chan;
452}
453
454static void l2cap_chan_destroy(struct kref *kref)
455{
456 struct l2cap_chan *chan = container_of(kref, struct l2cap_chan, kref);
457
458 BT_DBG("chan %p", chan);
459
460 write_lock(&chan_list_lock);
461 list_del(&chan->global_l);
462 write_unlock(&chan_list_lock);
463
464 kfree(chan);
465}
466
467void l2cap_chan_hold(struct l2cap_chan *c)
468{
469 BT_DBG("chan %p orig refcnt %d", c, atomic_read(&c->kref.refcount));
470
471 kref_get(&c->kref);
472}
473
474void l2cap_chan_put(struct l2cap_chan *c)
475{
476 BT_DBG("chan %p orig refcnt %d", c, atomic_read(&c->kref.refcount));
477
478 kref_put(&c->kref, l2cap_chan_destroy);
479}
480
481void l2cap_chan_set_defaults(struct l2cap_chan *chan)
482{
483 chan->fcs = L2CAP_FCS_CRC16;
484 chan->max_tx = L2CAP_DEFAULT_MAX_TX;
485 chan->tx_win = L2CAP_DEFAULT_TX_WINDOW;
486 chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW;
487 chan->ack_win = L2CAP_DEFAULT_TX_WINDOW;
488 chan->sec_level = BT_SECURITY_LOW;
489
490 set_bit(FLAG_FORCE_ACTIVE, &chan->flags);
491}
492
493void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
494{
495 BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn,
496 __le16_to_cpu(chan->psm), chan->dcid);
497
498 conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM;
499
500 chan->conn = conn;
501
502 switch (chan->chan_type) {
503 case L2CAP_CHAN_CONN_ORIENTED:
504 if (conn->hcon->type == LE_LINK) {
505
506 chan->omtu = L2CAP_DEFAULT_MTU;
507 chan->scid = L2CAP_CID_LE_DATA;
508 chan->dcid = L2CAP_CID_LE_DATA;
509 } else {
510
511 chan->scid = l2cap_alloc_cid(conn);
512 chan->omtu = L2CAP_DEFAULT_MTU;
513 }
514 break;
515
516 case L2CAP_CHAN_CONN_LESS:
517
518 chan->scid = L2CAP_CID_CONN_LESS;
519 chan->dcid = L2CAP_CID_CONN_LESS;
520 chan->omtu = L2CAP_DEFAULT_MTU;
521 break;
522
523 case L2CAP_CHAN_CONN_FIX_A2MP:
524 chan->scid = L2CAP_CID_A2MP;
525 chan->dcid = L2CAP_CID_A2MP;
526 chan->omtu = L2CAP_A2MP_DEFAULT_MTU;
527 chan->imtu = L2CAP_A2MP_DEFAULT_MTU;
528 break;
529
530 default:
531
532 chan->scid = L2CAP_CID_SIGNALING;
533 chan->dcid = L2CAP_CID_SIGNALING;
534 chan->omtu = L2CAP_DEFAULT_MTU;
535 }
536
537 chan->local_id = L2CAP_BESTEFFORT_ID;
538 chan->local_stype = L2CAP_SERV_BESTEFFORT;
539 chan->local_msdu = L2CAP_DEFAULT_MAX_SDU_SIZE;
540 chan->local_sdu_itime = L2CAP_DEFAULT_SDU_ITIME;
541 chan->local_acc_lat = L2CAP_DEFAULT_ACC_LAT;
542 chan->local_flush_to = L2CAP_EFS_DEFAULT_FLUSH_TO;
543
544 l2cap_chan_hold(chan);
545
546 list_add(&chan->list, &conn->chan_l);
547}
548
549void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
550{
551 mutex_lock(&conn->chan_lock);
552 __l2cap_chan_add(conn, chan);
553 mutex_unlock(&conn->chan_lock);
554}
555
556void l2cap_chan_del(struct l2cap_chan *chan, int err)
557{
558 struct l2cap_conn *conn = chan->conn;
559
560 __clear_chan_timer(chan);
561
562 BT_DBG("chan %p, conn %p, err %d", chan, conn, err);
563
564 if (conn) {
565 struct amp_mgr *mgr = conn->hcon->amp_mgr;
566
567 list_del(&chan->list);
568
569 l2cap_chan_put(chan);
570
571 chan->conn = NULL;
572
573 if (chan->chan_type != L2CAP_CHAN_CONN_FIX_A2MP)
574 hci_conn_drop(conn->hcon);
575
576 if (mgr && mgr->bredr_chan == chan)
577 mgr->bredr_chan = NULL;
578 }
579
580 if (chan->hs_hchan) {
581 struct hci_chan *hs_hchan = chan->hs_hchan;
582
583 BT_DBG("chan %p disconnect hs_hchan %p", chan, hs_hchan);
584 amp_disconnect_logical_link(hs_hchan);
585 }
586
587 chan->ops->teardown(chan, err);
588
589 if (test_bit(CONF_NOT_COMPLETE, &chan->conf_state))
590 return;
591
592 switch(chan->mode) {
593 case L2CAP_MODE_BASIC:
594 break;
595
596 case L2CAP_MODE_ERTM:
597 __clear_retrans_timer(chan);
598 __clear_monitor_timer(chan);
599 __clear_ack_timer(chan);
600
601 skb_queue_purge(&chan->srej_q);
602
603 l2cap_seq_list_free(&chan->srej_list);
604 l2cap_seq_list_free(&chan->retrans_list);
605
606
607
608 case L2CAP_MODE_STREAMING:
609 skb_queue_purge(&chan->tx_q);
610 break;
611 }
612
613 return;
614}
615
616void l2cap_chan_close(struct l2cap_chan *chan, int reason)
617{
618 struct l2cap_conn *conn = chan->conn;
619 struct sock *sk = chan->sk;
620
621 BT_DBG("chan %p state %s sk %p", chan, state_to_string(chan->state),
622 sk);
623
624 switch (chan->state) {
625 case BT_LISTEN:
626 chan->ops->teardown(chan, 0);
627 break;
628
629 case BT_CONNECTED:
630 case BT_CONFIG:
631 if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED &&
632 conn->hcon->type == ACL_LINK) {
633 __set_chan_timer(chan, sk->sk_sndtimeo);
634 l2cap_send_disconn_req(chan, reason);
635 } else
636 l2cap_chan_del(chan, reason);
637 break;
638
639 case BT_CONNECT2:
640 if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED &&
641 conn->hcon->type == ACL_LINK) {
642 struct l2cap_conn_rsp rsp;
643 __u16 result;
644
645 if (test_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags))
646 result = L2CAP_CR_SEC_BLOCK;
647 else
648 result = L2CAP_CR_BAD_PSM;
649 l2cap_state_change(chan, BT_DISCONN);
650
651 rsp.scid = cpu_to_le16(chan->dcid);
652 rsp.dcid = cpu_to_le16(chan->scid);
653 rsp.result = cpu_to_le16(result);
654 rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
655 l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
656 sizeof(rsp), &rsp);
657 }
658
659 l2cap_chan_del(chan, reason);
660 break;
661
662 case BT_CONNECT:
663 case BT_DISCONN:
664 l2cap_chan_del(chan, reason);
665 break;
666
667 default:
668 chan->ops->teardown(chan, 0);
669 break;
670 }
671}
672
673static inline u8 l2cap_get_auth_type(struct l2cap_chan *chan)
674{
675 if (chan->chan_type == L2CAP_CHAN_RAW) {
676 switch (chan->sec_level) {
677 case BT_SECURITY_HIGH:
678 return HCI_AT_DEDICATED_BONDING_MITM;
679 case BT_SECURITY_MEDIUM:
680 return HCI_AT_DEDICATED_BONDING;
681 default:
682 return HCI_AT_NO_BONDING;
683 }
684 } else if (chan->psm == __constant_cpu_to_le16(L2CAP_PSM_SDP)) {
685 if (chan->sec_level == BT_SECURITY_LOW)
686 chan->sec_level = BT_SECURITY_SDP;
687
688 if (chan->sec_level == BT_SECURITY_HIGH)
689 return HCI_AT_NO_BONDING_MITM;
690 else
691 return HCI_AT_NO_BONDING;
692 } else {
693 switch (chan->sec_level) {
694 case BT_SECURITY_HIGH:
695 return HCI_AT_GENERAL_BONDING_MITM;
696 case BT_SECURITY_MEDIUM:
697 return HCI_AT_GENERAL_BONDING;
698 default:
699 return HCI_AT_NO_BONDING;
700 }
701 }
702}
703
704
705int l2cap_chan_check_security(struct l2cap_chan *chan)
706{
707 struct l2cap_conn *conn = chan->conn;
708 __u8 auth_type;
709
710 auth_type = l2cap_get_auth_type(chan);
711
712 return hci_conn_security(conn->hcon, chan->sec_level, auth_type);
713}
714
715static u8 l2cap_get_ident(struct l2cap_conn *conn)
716{
717 u8 id;
718
719
720
721
722
723
724
725 spin_lock(&conn->lock);
726
727 if (++conn->tx_ident > 128)
728 conn->tx_ident = 1;
729
730 id = conn->tx_ident;
731
732 spin_unlock(&conn->lock);
733
734 return id;
735}
736
737static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len,
738 void *data)
739{
740 struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data);
741 u8 flags;
742
743 BT_DBG("code 0x%2.2x", code);
744
745 if (!skb)
746 return;
747
748 if (lmp_no_flush_capable(conn->hcon->hdev))
749 flags = ACL_START_NO_FLUSH;
750 else
751 flags = ACL_START;
752
753 bt_cb(skb)->force_active = BT_POWER_FORCE_ACTIVE_ON;
754 skb->priority = HCI_PRIO_MAX;
755
756 hci_send_acl(conn->hchan, skb, flags);
757}
758
759static bool __chan_is_moving(struct l2cap_chan *chan)
760{
761 return chan->move_state != L2CAP_MOVE_STABLE &&
762 chan->move_state != L2CAP_MOVE_WAIT_PREPARE;
763}
764
765static void l2cap_do_send(struct l2cap_chan *chan, struct sk_buff *skb)
766{
767 struct hci_conn *hcon = chan->conn->hcon;
768 u16 flags;
769
770 BT_DBG("chan %p, skb %p len %d priority %u", chan, skb, skb->len,
771 skb->priority);
772
773 if (chan->hs_hcon && !__chan_is_moving(chan)) {
774 if (chan->hs_hchan)
775 hci_send_acl(chan->hs_hchan, skb, ACL_COMPLETE);
776 else
777 kfree_skb(skb);
778
779 return;
780 }
781
782 if (!test_bit(FLAG_FLUSHABLE, &chan->flags) &&
783 lmp_no_flush_capable(hcon->hdev))
784 flags = ACL_START_NO_FLUSH;
785 else
786 flags = ACL_START;
787
788 bt_cb(skb)->force_active = test_bit(FLAG_FORCE_ACTIVE, &chan->flags);
789 hci_send_acl(chan->conn->hchan, skb, flags);
790}
791
792static void __unpack_enhanced_control(u16 enh, struct l2cap_ctrl *control)
793{
794 control->reqseq = (enh & L2CAP_CTRL_REQSEQ) >> L2CAP_CTRL_REQSEQ_SHIFT;
795 control->final = (enh & L2CAP_CTRL_FINAL) >> L2CAP_CTRL_FINAL_SHIFT;
796
797 if (enh & L2CAP_CTRL_FRAME_TYPE) {
798
799 control->sframe = 1;
800 control->poll = (enh & L2CAP_CTRL_POLL) >> L2CAP_CTRL_POLL_SHIFT;
801 control->super = (enh & L2CAP_CTRL_SUPERVISE) >> L2CAP_CTRL_SUPER_SHIFT;
802
803 control->sar = 0;
804 control->txseq = 0;
805 } else {
806
807 control->sframe = 0;
808 control->sar = (enh & L2CAP_CTRL_SAR) >> L2CAP_CTRL_SAR_SHIFT;
809 control->txseq = (enh & L2CAP_CTRL_TXSEQ) >> L2CAP_CTRL_TXSEQ_SHIFT;
810
811 control->poll = 0;
812 control->super = 0;
813 }
814}
815
816static void __unpack_extended_control(u32 ext, struct l2cap_ctrl *control)
817{
818 control->reqseq = (ext & L2CAP_EXT_CTRL_REQSEQ) >> L2CAP_EXT_CTRL_REQSEQ_SHIFT;
819 control->final = (ext & L2CAP_EXT_CTRL_FINAL) >> L2CAP_EXT_CTRL_FINAL_SHIFT;
820
821 if (ext & L2CAP_EXT_CTRL_FRAME_TYPE) {
822
823 control->sframe = 1;
824 control->poll = (ext & L2CAP_EXT_CTRL_POLL) >> L2CAP_EXT_CTRL_POLL_SHIFT;
825 control->super = (ext & L2CAP_EXT_CTRL_SUPERVISE) >> L2CAP_EXT_CTRL_SUPER_SHIFT;
826
827 control->sar = 0;
828 control->txseq = 0;
829 } else {
830
831 control->sframe = 0;
832 control->sar = (ext & L2CAP_EXT_CTRL_SAR) >> L2CAP_EXT_CTRL_SAR_SHIFT;
833 control->txseq = (ext & L2CAP_EXT_CTRL_TXSEQ) >> L2CAP_EXT_CTRL_TXSEQ_SHIFT;
834
835 control->poll = 0;
836 control->super = 0;
837 }
838}
839
840static inline void __unpack_control(struct l2cap_chan *chan,
841 struct sk_buff *skb)
842{
843 if (test_bit(FLAG_EXT_CTRL, &chan->flags)) {
844 __unpack_extended_control(get_unaligned_le32(skb->data),
845 &bt_cb(skb)->control);
846 skb_pull(skb, L2CAP_EXT_CTRL_SIZE);
847 } else {
848 __unpack_enhanced_control(get_unaligned_le16(skb->data),
849 &bt_cb(skb)->control);
850 skb_pull(skb, L2CAP_ENH_CTRL_SIZE);
851 }
852}
853
854static u32 __pack_extended_control(struct l2cap_ctrl *control)
855{
856 u32 packed;
857
858 packed = control->reqseq << L2CAP_EXT_CTRL_REQSEQ_SHIFT;
859 packed |= control->final << L2CAP_EXT_CTRL_FINAL_SHIFT;
860
861 if (control->sframe) {
862 packed |= control->poll << L2CAP_EXT_CTRL_POLL_SHIFT;
863 packed |= control->super << L2CAP_EXT_CTRL_SUPER_SHIFT;
864 packed |= L2CAP_EXT_CTRL_FRAME_TYPE;
865 } else {
866 packed |= control->sar << L2CAP_EXT_CTRL_SAR_SHIFT;
867 packed |= control->txseq << L2CAP_EXT_CTRL_TXSEQ_SHIFT;
868 }
869
870 return packed;
871}
872
873static u16 __pack_enhanced_control(struct l2cap_ctrl *control)
874{
875 u16 packed;
876
877 packed = control->reqseq << L2CAP_CTRL_REQSEQ_SHIFT;
878 packed |= control->final << L2CAP_CTRL_FINAL_SHIFT;
879
880 if (control->sframe) {
881 packed |= control->poll << L2CAP_CTRL_POLL_SHIFT;
882 packed |= control->super << L2CAP_CTRL_SUPER_SHIFT;
883 packed |= L2CAP_CTRL_FRAME_TYPE;
884 } else {
885 packed |= control->sar << L2CAP_CTRL_SAR_SHIFT;
886 packed |= control->txseq << L2CAP_CTRL_TXSEQ_SHIFT;
887 }
888
889 return packed;
890}
891
892static inline void __pack_control(struct l2cap_chan *chan,
893 struct l2cap_ctrl *control,
894 struct sk_buff *skb)
895{
896 if (test_bit(FLAG_EXT_CTRL, &chan->flags)) {
897 put_unaligned_le32(__pack_extended_control(control),
898 skb->data + L2CAP_HDR_SIZE);
899 } else {
900 put_unaligned_le16(__pack_enhanced_control(control),
901 skb->data + L2CAP_HDR_SIZE);
902 }
903}
904
905static inline unsigned int __ertm_hdr_size(struct l2cap_chan *chan)
906{
907 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
908 return L2CAP_EXT_HDR_SIZE;
909 else
910 return L2CAP_ENH_HDR_SIZE;
911}
912
913static struct sk_buff *l2cap_create_sframe_pdu(struct l2cap_chan *chan,
914 u32 control)
915{
916 struct sk_buff *skb;
917 struct l2cap_hdr *lh;
918 int hlen = __ertm_hdr_size(chan);
919
920 if (chan->fcs == L2CAP_FCS_CRC16)
921 hlen += L2CAP_FCS_SIZE;
922
923 skb = bt_skb_alloc(hlen, GFP_KERNEL);
924
925 if (!skb)
926 return ERR_PTR(-ENOMEM);
927
928 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
929 lh->len = cpu_to_le16(hlen - L2CAP_HDR_SIZE);
930 lh->cid = cpu_to_le16(chan->dcid);
931
932 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
933 put_unaligned_le32(control, skb_put(skb, L2CAP_EXT_CTRL_SIZE));
934 else
935 put_unaligned_le16(control, skb_put(skb, L2CAP_ENH_CTRL_SIZE));
936
937 if (chan->fcs == L2CAP_FCS_CRC16) {
938 u16 fcs = crc16(0, (u8 *)skb->data, skb->len);
939 put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE));
940 }
941
942 skb->priority = HCI_PRIO_MAX;
943 return skb;
944}
945
946static void l2cap_send_sframe(struct l2cap_chan *chan,
947 struct l2cap_ctrl *control)
948{
949 struct sk_buff *skb;
950 u32 control_field;
951
952 BT_DBG("chan %p, control %p", chan, control);
953
954 if (!control->sframe)
955 return;
956
957 if (__chan_is_moving(chan))
958 return;
959
960 if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state) &&
961 !control->poll)
962 control->final = 1;
963
964 if (control->super == L2CAP_SUPER_RR)
965 clear_bit(CONN_RNR_SENT, &chan->conn_state);
966 else if (control->super == L2CAP_SUPER_RNR)
967 set_bit(CONN_RNR_SENT, &chan->conn_state);
968
969 if (control->super != L2CAP_SUPER_SREJ) {
970 chan->last_acked_seq = control->reqseq;
971 __clear_ack_timer(chan);
972 }
973
974 BT_DBG("reqseq %d, final %d, poll %d, super %d", control->reqseq,
975 control->final, control->poll, control->super);
976
977 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
978 control_field = __pack_extended_control(control);
979 else
980 control_field = __pack_enhanced_control(control);
981
982 skb = l2cap_create_sframe_pdu(chan, control_field);
983 if (!IS_ERR(skb))
984 l2cap_do_send(chan, skb);
985}
986
987static void l2cap_send_rr_or_rnr(struct l2cap_chan *chan, bool poll)
988{
989 struct l2cap_ctrl control;
990
991 BT_DBG("chan %p, poll %d", chan, poll);
992
993 memset(&control, 0, sizeof(control));
994 control.sframe = 1;
995 control.poll = poll;
996
997 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state))
998 control.super = L2CAP_SUPER_RNR;
999 else
1000 control.super = L2CAP_SUPER_RR;
1001
1002 control.reqseq = chan->buffer_seq;
1003 l2cap_send_sframe(chan, &control);
1004}
1005
1006static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan)
1007{
1008 return !test_bit(CONF_CONNECT_PEND, &chan->conf_state);
1009}
1010
1011static bool __amp_capable(struct l2cap_chan *chan)
1012{
1013 struct l2cap_conn *conn = chan->conn;
1014
1015 if (enable_hs &&
1016 hci_amp_capable() &&
1017 chan->chan_policy == BT_CHANNEL_POLICY_AMP_PREFERRED &&
1018 conn->fixed_chan_mask & L2CAP_FC_A2MP)
1019 return true;
1020 else
1021 return false;
1022}
1023
1024static bool l2cap_check_efs(struct l2cap_chan *chan)
1025{
1026
1027 return true;
1028}
1029
1030void l2cap_send_conn_req(struct l2cap_chan *chan)
1031{
1032 struct l2cap_conn *conn = chan->conn;
1033 struct l2cap_conn_req req;
1034
1035 req.scid = cpu_to_le16(chan->scid);
1036 req.psm = chan->psm;
1037
1038 chan->ident = l2cap_get_ident(conn);
1039
1040 set_bit(CONF_CONNECT_PEND, &chan->conf_state);
1041
1042 l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_REQ, sizeof(req), &req);
1043}
1044
1045static void l2cap_send_create_chan_req(struct l2cap_chan *chan, u8 amp_id)
1046{
1047 struct l2cap_create_chan_req req;
1048 req.scid = cpu_to_le16(chan->scid);
1049 req.psm = chan->psm;
1050 req.amp_id = amp_id;
1051
1052 chan->ident = l2cap_get_ident(chan->conn);
1053
1054 l2cap_send_cmd(chan->conn, chan->ident, L2CAP_CREATE_CHAN_REQ,
1055 sizeof(req), &req);
1056}
1057
1058static void l2cap_move_setup(struct l2cap_chan *chan)
1059{
1060 struct sk_buff *skb;
1061
1062 BT_DBG("chan %p", chan);
1063
1064 if (chan->mode != L2CAP_MODE_ERTM)
1065 return;
1066
1067 __clear_retrans_timer(chan);
1068 __clear_monitor_timer(chan);
1069 __clear_ack_timer(chan);
1070
1071 chan->retry_count = 0;
1072 skb_queue_walk(&chan->tx_q, skb) {
1073 if (bt_cb(skb)->control.retries)
1074 bt_cb(skb)->control.retries = 1;
1075 else
1076 break;
1077 }
1078
1079 chan->expected_tx_seq = chan->buffer_seq;
1080
1081 clear_bit(CONN_REJ_ACT, &chan->conn_state);
1082 clear_bit(CONN_SREJ_ACT, &chan->conn_state);
1083 l2cap_seq_list_clear(&chan->retrans_list);
1084 l2cap_seq_list_clear(&chan->srej_list);
1085 skb_queue_purge(&chan->srej_q);
1086
1087 chan->tx_state = L2CAP_TX_STATE_XMIT;
1088 chan->rx_state = L2CAP_RX_STATE_MOVE;
1089
1090 set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
1091}
1092
1093static void l2cap_move_done(struct l2cap_chan *chan)
1094{
1095 u8 move_role = chan->move_role;
1096 BT_DBG("chan %p", chan);
1097
1098 chan->move_state = L2CAP_MOVE_STABLE;
1099 chan->move_role = L2CAP_MOVE_ROLE_NONE;
1100
1101 if (chan->mode != L2CAP_MODE_ERTM)
1102 return;
1103
1104 switch (move_role) {
1105 case L2CAP_MOVE_ROLE_INITIATOR:
1106 l2cap_tx(chan, NULL, NULL, L2CAP_EV_EXPLICIT_POLL);
1107 chan->rx_state = L2CAP_RX_STATE_WAIT_F;
1108 break;
1109 case L2CAP_MOVE_ROLE_RESPONDER:
1110 chan->rx_state = L2CAP_RX_STATE_WAIT_P;
1111 break;
1112 }
1113}
1114
1115static void l2cap_chan_ready(struct l2cap_chan *chan)
1116{
1117
1118 chan->conf_state = 0;
1119 __clear_chan_timer(chan);
1120
1121 chan->state = BT_CONNECTED;
1122
1123 chan->ops->ready(chan);
1124}
1125
1126static void l2cap_start_connection(struct l2cap_chan *chan)
1127{
1128 if (__amp_capable(chan)) {
1129 BT_DBG("chan %p AMP capable: discover AMPs", chan);
1130 a2mp_discover_amp(chan);
1131 } else {
1132 l2cap_send_conn_req(chan);
1133 }
1134}
1135
1136static void l2cap_do_start(struct l2cap_chan *chan)
1137{
1138 struct l2cap_conn *conn = chan->conn;
1139
1140 if (conn->hcon->type == LE_LINK) {
1141 l2cap_chan_ready(chan);
1142 return;
1143 }
1144
1145 if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) {
1146 if (!(conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE))
1147 return;
1148
1149 if (l2cap_chan_check_security(chan) &&
1150 __l2cap_no_conn_pending(chan)) {
1151 l2cap_start_connection(chan);
1152 }
1153 } else {
1154 struct l2cap_info_req req;
1155 req.type = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
1156
1157 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
1158 conn->info_ident = l2cap_get_ident(conn);
1159
1160 schedule_delayed_work(&conn->info_timer, L2CAP_INFO_TIMEOUT);
1161
1162 l2cap_send_cmd(conn, conn->info_ident, L2CAP_INFO_REQ,
1163 sizeof(req), &req);
1164 }
1165}
1166
1167static inline int l2cap_mode_supported(__u8 mode, __u32 feat_mask)
1168{
1169 u32 local_feat_mask = l2cap_feat_mask;
1170 if (!disable_ertm)
1171 local_feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING;
1172
1173 switch (mode) {
1174 case L2CAP_MODE_ERTM:
1175 return L2CAP_FEAT_ERTM & feat_mask & local_feat_mask;
1176 case L2CAP_MODE_STREAMING:
1177 return L2CAP_FEAT_STREAMING & feat_mask & local_feat_mask;
1178 default:
1179 return 0x00;
1180 }
1181}
1182
1183static void l2cap_send_disconn_req(struct l2cap_chan *chan, int err)
1184{
1185 struct sock *sk = chan->sk;
1186 struct l2cap_conn *conn = chan->conn;
1187 struct l2cap_disconn_req req;
1188
1189 if (!conn)
1190 return;
1191
1192 if (chan->mode == L2CAP_MODE_ERTM && chan->state == BT_CONNECTED) {
1193 __clear_retrans_timer(chan);
1194 __clear_monitor_timer(chan);
1195 __clear_ack_timer(chan);
1196 }
1197
1198 if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP) {
1199 l2cap_state_change(chan, BT_DISCONN);
1200 return;
1201 }
1202
1203 req.dcid = cpu_to_le16(chan->dcid);
1204 req.scid = cpu_to_le16(chan->scid);
1205 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_DISCONN_REQ,
1206 sizeof(req), &req);
1207
1208 lock_sock(sk);
1209 __l2cap_state_change(chan, BT_DISCONN);
1210 __l2cap_chan_set_err(chan, err);
1211 release_sock(sk);
1212}
1213
1214
1215static void l2cap_conn_start(struct l2cap_conn *conn)
1216{
1217 struct l2cap_chan *chan, *tmp;
1218
1219 BT_DBG("conn %p", conn);
1220
1221 mutex_lock(&conn->chan_lock);
1222
1223 list_for_each_entry_safe(chan, tmp, &conn->chan_l, list) {
1224 struct sock *sk = chan->sk;
1225
1226 l2cap_chan_lock(chan);
1227
1228 if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
1229 l2cap_chan_unlock(chan);
1230 continue;
1231 }
1232
1233 if (chan->state == BT_CONNECT) {
1234 if (!l2cap_chan_check_security(chan) ||
1235 !__l2cap_no_conn_pending(chan)) {
1236 l2cap_chan_unlock(chan);
1237 continue;
1238 }
1239
1240 if (!l2cap_mode_supported(chan->mode, conn->feat_mask)
1241 && test_bit(CONF_STATE2_DEVICE,
1242 &chan->conf_state)) {
1243 l2cap_chan_close(chan, ECONNRESET);
1244 l2cap_chan_unlock(chan);
1245 continue;
1246 }
1247
1248 l2cap_start_connection(chan);
1249
1250 } else if (chan->state == BT_CONNECT2) {
1251 struct l2cap_conn_rsp rsp;
1252 char buf[128];
1253 rsp.scid = cpu_to_le16(chan->dcid);
1254 rsp.dcid = cpu_to_le16(chan->scid);
1255
1256 if (l2cap_chan_check_security(chan)) {
1257 lock_sock(sk);
1258 if (test_bit(BT_SK_DEFER_SETUP,
1259 &bt_sk(sk)->flags)) {
1260 rsp.result = __constant_cpu_to_le16(L2CAP_CR_PEND);
1261 rsp.status = __constant_cpu_to_le16(L2CAP_CS_AUTHOR_PEND);
1262 chan->ops->defer(chan);
1263
1264 } else {
1265 __l2cap_state_change(chan, BT_CONFIG);
1266 rsp.result = __constant_cpu_to_le16(L2CAP_CR_SUCCESS);
1267 rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
1268 }
1269 release_sock(sk);
1270 } else {
1271 rsp.result = __constant_cpu_to_le16(L2CAP_CR_PEND);
1272 rsp.status = __constant_cpu_to_le16(L2CAP_CS_AUTHEN_PEND);
1273 }
1274
1275 l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
1276 sizeof(rsp), &rsp);
1277
1278 if (test_bit(CONF_REQ_SENT, &chan->conf_state) ||
1279 rsp.result != L2CAP_CR_SUCCESS) {
1280 l2cap_chan_unlock(chan);
1281 continue;
1282 }
1283
1284 set_bit(CONF_REQ_SENT, &chan->conf_state);
1285 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
1286 l2cap_build_conf_req(chan, buf), buf);
1287 chan->num_conf_req++;
1288 }
1289
1290 l2cap_chan_unlock(chan);
1291 }
1292
1293 mutex_unlock(&conn->chan_lock);
1294}
1295
1296
1297
1298
1299static struct l2cap_chan *l2cap_global_chan_by_scid(int state, u16 cid,
1300 bdaddr_t *src,
1301 bdaddr_t *dst)
1302{
1303 struct l2cap_chan *c, *c1 = NULL;
1304
1305 read_lock(&chan_list_lock);
1306
1307 list_for_each_entry(c, &chan_list, global_l) {
1308 struct sock *sk = c->sk;
1309
1310 if (state && c->state != state)
1311 continue;
1312
1313 if (c->scid == cid) {
1314 int src_match, dst_match;
1315 int src_any, dst_any;
1316
1317
1318 src_match = !bacmp(&bt_sk(sk)->src, src);
1319 dst_match = !bacmp(&bt_sk(sk)->dst, dst);
1320 if (src_match && dst_match) {
1321 read_unlock(&chan_list_lock);
1322 return c;
1323 }
1324
1325
1326 src_any = !bacmp(&bt_sk(sk)->src, BDADDR_ANY);
1327 dst_any = !bacmp(&bt_sk(sk)->dst, BDADDR_ANY);
1328 if ((src_match && dst_any) || (src_any && dst_match) ||
1329 (src_any && dst_any))
1330 c1 = c;
1331 }
1332 }
1333
1334 read_unlock(&chan_list_lock);
1335
1336 return c1;
1337}
1338
1339static void l2cap_le_conn_ready(struct l2cap_conn *conn)
1340{
1341 struct sock *parent, *sk;
1342 struct l2cap_chan *chan, *pchan;
1343
1344 BT_DBG("");
1345
1346
1347 pchan = l2cap_global_chan_by_scid(BT_LISTEN, L2CAP_CID_LE_DATA,
1348 conn->src, conn->dst);
1349 if (!pchan)
1350 return;
1351
1352 parent = pchan->sk;
1353
1354 lock_sock(parent);
1355
1356 chan = pchan->ops->new_connection(pchan);
1357 if (!chan)
1358 goto clean;
1359
1360 sk = chan->sk;
1361
1362 hci_conn_hold(conn->hcon);
1363 conn->hcon->disc_timeout = HCI_DISCONN_TIMEOUT;
1364
1365 bacpy(&bt_sk(sk)->src, conn->src);
1366 bacpy(&bt_sk(sk)->dst, conn->dst);
1367
1368 l2cap_chan_add(conn, chan);
1369
1370 l2cap_chan_ready(chan);
1371
1372clean:
1373 release_sock(parent);
1374}
1375
1376static void l2cap_conn_ready(struct l2cap_conn *conn)
1377{
1378 struct l2cap_chan *chan;
1379 struct hci_conn *hcon = conn->hcon;
1380
1381 BT_DBG("conn %p", conn);
1382
1383 if (!hcon->out && hcon->type == LE_LINK)
1384 l2cap_le_conn_ready(conn);
1385
1386 if (hcon->out && hcon->type == LE_LINK)
1387 smp_conn_security(hcon, hcon->pending_sec_level);
1388
1389 mutex_lock(&conn->chan_lock);
1390
1391 list_for_each_entry(chan, &conn->chan_l, list) {
1392
1393 l2cap_chan_lock(chan);
1394
1395 if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP) {
1396 l2cap_chan_unlock(chan);
1397 continue;
1398 }
1399
1400 if (hcon->type == LE_LINK) {
1401 if (smp_conn_security(hcon, chan->sec_level))
1402 l2cap_chan_ready(chan);
1403
1404 } else if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
1405 struct sock *sk = chan->sk;
1406 __clear_chan_timer(chan);
1407 lock_sock(sk);
1408 __l2cap_state_change(chan, BT_CONNECTED);
1409 sk->sk_state_change(sk);
1410 release_sock(sk);
1411
1412 } else if (chan->state == BT_CONNECT)
1413 l2cap_do_start(chan);
1414
1415 l2cap_chan_unlock(chan);
1416 }
1417
1418 mutex_unlock(&conn->chan_lock);
1419}
1420
1421
1422static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err)
1423{
1424 struct l2cap_chan *chan;
1425
1426 BT_DBG("conn %p", conn);
1427
1428 mutex_lock(&conn->chan_lock);
1429
1430 list_for_each_entry(chan, &conn->chan_l, list) {
1431 if (test_bit(FLAG_FORCE_RELIABLE, &chan->flags))
1432 l2cap_chan_set_err(chan, err);
1433 }
1434
1435 mutex_unlock(&conn->chan_lock);
1436}
1437
1438static void l2cap_info_timeout(struct work_struct *work)
1439{
1440 struct l2cap_conn *conn = container_of(work, struct l2cap_conn,
1441 info_timer.work);
1442
1443 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
1444 conn->info_ident = 0;
1445
1446 l2cap_conn_start(conn);
1447}
1448
1449
1450
1451
1452
1453
1454
1455
1456
1457
1458
1459
1460
1461
1462int l2cap_register_user(struct l2cap_conn *conn, struct l2cap_user *user)
1463{
1464 struct hci_dev *hdev = conn->hcon->hdev;
1465 int ret;
1466
1467
1468
1469
1470
1471
1472
1473
1474 hci_dev_lock(hdev);
1475
1476 if (user->list.next || user->list.prev) {
1477 ret = -EINVAL;
1478 goto out_unlock;
1479 }
1480
1481
1482 if (!conn->hchan) {
1483 ret = -ENODEV;
1484 goto out_unlock;
1485 }
1486
1487 ret = user->probe(conn, user);
1488 if (ret)
1489 goto out_unlock;
1490
1491 list_add(&user->list, &conn->users);
1492 ret = 0;
1493
1494out_unlock:
1495 hci_dev_unlock(hdev);
1496 return ret;
1497}
1498EXPORT_SYMBOL(l2cap_register_user);
1499
1500void l2cap_unregister_user(struct l2cap_conn *conn, struct l2cap_user *user)
1501{
1502 struct hci_dev *hdev = conn->hcon->hdev;
1503
1504 hci_dev_lock(hdev);
1505
1506 if (!user->list.next || !user->list.prev)
1507 goto out_unlock;
1508
1509 list_del(&user->list);
1510 user->list.next = NULL;
1511 user->list.prev = NULL;
1512 user->remove(conn, user);
1513
1514out_unlock:
1515 hci_dev_unlock(hdev);
1516}
1517EXPORT_SYMBOL(l2cap_unregister_user);
1518
1519static void l2cap_unregister_all_users(struct l2cap_conn *conn)
1520{
1521 struct l2cap_user *user;
1522
1523 while (!list_empty(&conn->users)) {
1524 user = list_first_entry(&conn->users, struct l2cap_user, list);
1525 list_del(&user->list);
1526 user->list.next = NULL;
1527 user->list.prev = NULL;
1528 user->remove(conn, user);
1529 }
1530}
1531
1532static void l2cap_conn_del(struct hci_conn *hcon, int err)
1533{
1534 struct l2cap_conn *conn = hcon->l2cap_data;
1535 struct l2cap_chan *chan, *l;
1536
1537 if (!conn)
1538 return;
1539
1540 BT_DBG("hcon %p conn %p, err %d", hcon, conn, err);
1541
1542 kfree_skb(conn->rx_skb);
1543
1544 l2cap_unregister_all_users(conn);
1545
1546 mutex_lock(&conn->chan_lock);
1547
1548
1549 list_for_each_entry_safe(chan, l, &conn->chan_l, list) {
1550 l2cap_chan_hold(chan);
1551 l2cap_chan_lock(chan);
1552
1553 l2cap_chan_del(chan, err);
1554
1555 l2cap_chan_unlock(chan);
1556
1557 chan->ops->close(chan);
1558 l2cap_chan_put(chan);
1559 }
1560
1561 mutex_unlock(&conn->chan_lock);
1562
1563 hci_chan_del(conn->hchan);
1564
1565 if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT)
1566 cancel_delayed_work_sync(&conn->info_timer);
1567
1568 if (test_and_clear_bit(HCI_CONN_LE_SMP_PEND, &hcon->flags)) {
1569 cancel_delayed_work_sync(&conn->security_timer);
1570 smp_chan_destroy(conn);
1571 }
1572
1573 hcon->l2cap_data = NULL;
1574 conn->hchan = NULL;
1575 l2cap_conn_put(conn);
1576}
1577
1578static void security_timeout(struct work_struct *work)
1579{
1580 struct l2cap_conn *conn = container_of(work, struct l2cap_conn,
1581 security_timer.work);
1582
1583 BT_DBG("conn %p", conn);
1584
1585 if (test_and_clear_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->flags)) {
1586 smp_chan_destroy(conn);
1587 l2cap_conn_del(conn->hcon, ETIMEDOUT);
1588 }
1589}
1590
1591static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon)
1592{
1593 struct l2cap_conn *conn = hcon->l2cap_data;
1594 struct hci_chan *hchan;
1595
1596 if (conn)
1597 return conn;
1598
1599 hchan = hci_chan_create(hcon);
1600 if (!hchan)
1601 return NULL;
1602
1603 conn = kzalloc(sizeof(struct l2cap_conn), GFP_KERNEL);
1604 if (!conn) {
1605 hci_chan_del(hchan);
1606 return NULL;
1607 }
1608
1609 kref_init(&conn->ref);
1610 hcon->l2cap_data = conn;
1611 conn->hcon = hcon;
1612 hci_conn_get(conn->hcon);
1613 conn->hchan = hchan;
1614
1615 BT_DBG("hcon %p conn %p hchan %p", hcon, conn, hchan);
1616
1617 switch (hcon->type) {
1618 case LE_LINK:
1619 if (hcon->hdev->le_mtu) {
1620 conn->mtu = hcon->hdev->le_mtu;
1621 break;
1622 }
1623
1624 default:
1625 conn->mtu = hcon->hdev->acl_mtu;
1626 break;
1627 }
1628
1629 conn->src = &hcon->hdev->bdaddr;
1630 conn->dst = &hcon->dst;
1631
1632 conn->feat_mask = 0;
1633
1634 spin_lock_init(&conn->lock);
1635 mutex_init(&conn->chan_lock);
1636
1637 INIT_LIST_HEAD(&conn->chan_l);
1638 INIT_LIST_HEAD(&conn->users);
1639
1640 if (hcon->type == LE_LINK)
1641 INIT_DELAYED_WORK(&conn->security_timer, security_timeout);
1642 else
1643 INIT_DELAYED_WORK(&conn->info_timer, l2cap_info_timeout);
1644
1645 conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM;
1646
1647 return conn;
1648}
1649
1650static void l2cap_conn_free(struct kref *ref)
1651{
1652 struct l2cap_conn *conn = container_of(ref, struct l2cap_conn, ref);
1653
1654 hci_conn_put(conn->hcon);
1655 kfree(conn);
1656}
1657
1658void l2cap_conn_get(struct l2cap_conn *conn)
1659{
1660 kref_get(&conn->ref);
1661}
1662EXPORT_SYMBOL(l2cap_conn_get);
1663
1664void l2cap_conn_put(struct l2cap_conn *conn)
1665{
1666 kref_put(&conn->ref, l2cap_conn_free);
1667}
1668EXPORT_SYMBOL(l2cap_conn_put);
1669
1670
1671
1672
1673
1674
1675static struct l2cap_chan *l2cap_global_chan_by_psm(int state, __le16 psm,
1676 bdaddr_t *src,
1677 bdaddr_t *dst)
1678{
1679 struct l2cap_chan *c, *c1 = NULL;
1680
1681 read_lock(&chan_list_lock);
1682
1683 list_for_each_entry(c, &chan_list, global_l) {
1684 struct sock *sk = c->sk;
1685
1686 if (state && c->state != state)
1687 continue;
1688
1689 if (c->psm == psm) {
1690 int src_match, dst_match;
1691 int src_any, dst_any;
1692
1693
1694 src_match = !bacmp(&bt_sk(sk)->src, src);
1695 dst_match = !bacmp(&bt_sk(sk)->dst, dst);
1696 if (src_match && dst_match) {
1697 read_unlock(&chan_list_lock);
1698 return c;
1699 }
1700
1701
1702 src_any = !bacmp(&bt_sk(sk)->src, BDADDR_ANY);
1703 dst_any = !bacmp(&bt_sk(sk)->dst, BDADDR_ANY);
1704 if ((src_match && dst_any) || (src_any && dst_match) ||
1705 (src_any && dst_any))
1706 c1 = c;
1707 }
1708 }
1709
1710 read_unlock(&chan_list_lock);
1711
1712 return c1;
1713}
1714
1715int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid,
1716 bdaddr_t *dst, u8 dst_type)
1717{
1718 struct sock *sk = chan->sk;
1719 bdaddr_t *src = &bt_sk(sk)->src;
1720 struct l2cap_conn *conn;
1721 struct hci_conn *hcon;
1722 struct hci_dev *hdev;
1723 __u8 auth_type;
1724 int err;
1725
1726 BT_DBG("%pMR -> %pMR (type %u) psm 0x%2.2x", src, dst,
1727 dst_type, __le16_to_cpu(psm));
1728
1729 hdev = hci_get_route(dst, src);
1730 if (!hdev)
1731 return -EHOSTUNREACH;
1732
1733 hci_dev_lock(hdev);
1734
1735 l2cap_chan_lock(chan);
1736
1737
1738 if ((__le16_to_cpu(psm) & 0x0101) != 0x0001 && !cid &&
1739 chan->chan_type != L2CAP_CHAN_RAW) {
1740 err = -EINVAL;
1741 goto done;
1742 }
1743
1744 if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED && !(psm || cid)) {
1745 err = -EINVAL;
1746 goto done;
1747 }
1748
1749 switch (chan->mode) {
1750 case L2CAP_MODE_BASIC:
1751 break;
1752 case L2CAP_MODE_ERTM:
1753 case L2CAP_MODE_STREAMING:
1754 if (!disable_ertm)
1755 break;
1756
1757 default:
1758 err = -ENOTSUPP;
1759 goto done;
1760 }
1761
1762 switch (chan->state) {
1763 case BT_CONNECT:
1764 case BT_CONNECT2:
1765 case BT_CONFIG:
1766
1767 err = 0;
1768 goto done;
1769
1770 case BT_CONNECTED:
1771
1772 err = -EISCONN;
1773 goto done;
1774
1775 case BT_OPEN:
1776 case BT_BOUND:
1777
1778 break;
1779
1780 default:
1781 err = -EBADFD;
1782 goto done;
1783 }
1784
1785
1786 lock_sock(sk);
1787 bacpy(&bt_sk(sk)->dst, dst);
1788 release_sock(sk);
1789
1790 chan->psm = psm;
1791 chan->dcid = cid;
1792
1793 auth_type = l2cap_get_auth_type(chan);
1794
1795 if (chan->dcid == L2CAP_CID_LE_DATA)
1796 hcon = hci_connect(hdev, LE_LINK, dst, dst_type,
1797 chan->sec_level, auth_type);
1798 else
1799 hcon = hci_connect(hdev, ACL_LINK, dst, dst_type,
1800 chan->sec_level, auth_type);
1801
1802 if (IS_ERR(hcon)) {
1803 err = PTR_ERR(hcon);
1804 goto done;
1805 }
1806
1807 conn = l2cap_conn_add(hcon);
1808 if (!conn) {
1809 hci_conn_drop(hcon);
1810 err = -ENOMEM;
1811 goto done;
1812 }
1813
1814 if (hcon->type == LE_LINK) {
1815 err = 0;
1816
1817 if (!list_empty(&conn->chan_l)) {
1818 err = -EBUSY;
1819 hci_conn_drop(hcon);
1820 }
1821
1822 if (err)
1823 goto done;
1824 }
1825
1826
1827 bacpy(src, conn->src);
1828
1829 l2cap_chan_unlock(chan);
1830 l2cap_chan_add(conn, chan);
1831 l2cap_chan_lock(chan);
1832
1833 l2cap_state_change(chan, BT_CONNECT);
1834 __set_chan_timer(chan, sk->sk_sndtimeo);
1835
1836 if (hcon->state == BT_CONNECTED) {
1837 if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
1838 __clear_chan_timer(chan);
1839 if (l2cap_chan_check_security(chan))
1840 l2cap_state_change(chan, BT_CONNECTED);
1841 } else
1842 l2cap_do_start(chan);
1843 }
1844
1845 err = 0;
1846
1847done:
1848 l2cap_chan_unlock(chan);
1849 hci_dev_unlock(hdev);
1850 hci_dev_put(hdev);
1851 return err;
1852}
1853
1854int __l2cap_wait_ack(struct sock *sk)
1855{
1856 struct l2cap_chan *chan = l2cap_pi(sk)->chan;
1857 DECLARE_WAITQUEUE(wait, current);
1858 int err = 0;
1859 int timeo = HZ/5;
1860
1861 add_wait_queue(sk_sleep(sk), &wait);
1862 set_current_state(TASK_INTERRUPTIBLE);
1863 while (chan->unacked_frames > 0 && chan->conn) {
1864 if (!timeo)
1865 timeo = HZ/5;
1866
1867 if (signal_pending(current)) {
1868 err = sock_intr_errno(timeo);
1869 break;
1870 }
1871
1872 release_sock(sk);
1873 timeo = schedule_timeout(timeo);
1874 lock_sock(sk);
1875 set_current_state(TASK_INTERRUPTIBLE);
1876
1877 err = sock_error(sk);
1878 if (err)
1879 break;
1880 }
1881 set_current_state(TASK_RUNNING);
1882 remove_wait_queue(sk_sleep(sk), &wait);
1883 return err;
1884}
1885
1886static void l2cap_monitor_timeout(struct work_struct *work)
1887{
1888 struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
1889 monitor_timer.work);
1890
1891 BT_DBG("chan %p", chan);
1892
1893 l2cap_chan_lock(chan);
1894
1895 if (!chan->conn) {
1896 l2cap_chan_unlock(chan);
1897 l2cap_chan_put(chan);
1898 return;
1899 }
1900
1901 l2cap_tx(chan, NULL, NULL, L2CAP_EV_MONITOR_TO);
1902
1903 l2cap_chan_unlock(chan);
1904 l2cap_chan_put(chan);
1905}
1906
1907static void l2cap_retrans_timeout(struct work_struct *work)
1908{
1909 struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
1910 retrans_timer.work);
1911
1912 BT_DBG("chan %p", chan);
1913
1914 l2cap_chan_lock(chan);
1915
1916 if (!chan->conn) {
1917 l2cap_chan_unlock(chan);
1918 l2cap_chan_put(chan);
1919 return;
1920 }
1921
1922 l2cap_tx(chan, NULL, NULL, L2CAP_EV_RETRANS_TO);
1923 l2cap_chan_unlock(chan);
1924 l2cap_chan_put(chan);
1925}
1926
1927static void l2cap_streaming_send(struct l2cap_chan *chan,
1928 struct sk_buff_head *skbs)
1929{
1930 struct sk_buff *skb;
1931 struct l2cap_ctrl *control;
1932
1933 BT_DBG("chan %p, skbs %p", chan, skbs);
1934
1935 if (__chan_is_moving(chan))
1936 return;
1937
1938 skb_queue_splice_tail_init(skbs, &chan->tx_q);
1939
1940 while (!skb_queue_empty(&chan->tx_q)) {
1941
1942 skb = skb_dequeue(&chan->tx_q);
1943
1944 bt_cb(skb)->control.retries = 1;
1945 control = &bt_cb(skb)->control;
1946
1947 control->reqseq = 0;
1948 control->txseq = chan->next_tx_seq;
1949
1950 __pack_control(chan, control, skb);
1951
1952 if (chan->fcs == L2CAP_FCS_CRC16) {
1953 u16 fcs = crc16(0, (u8 *) skb->data, skb->len);
1954 put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE));
1955 }
1956
1957 l2cap_do_send(chan, skb);
1958
1959 BT_DBG("Sent txseq %u", control->txseq);
1960
1961 chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq);
1962 chan->frames_sent++;
1963 }
1964}
1965
1966static int l2cap_ertm_send(struct l2cap_chan *chan)
1967{
1968 struct sk_buff *skb, *tx_skb;
1969 struct l2cap_ctrl *control;
1970 int sent = 0;
1971
1972 BT_DBG("chan %p", chan);
1973
1974 if (chan->state != BT_CONNECTED)
1975 return -ENOTCONN;
1976
1977 if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
1978 return 0;
1979
1980 if (__chan_is_moving(chan))
1981 return 0;
1982
1983 while (chan->tx_send_head &&
1984 chan->unacked_frames < chan->remote_tx_win &&
1985 chan->tx_state == L2CAP_TX_STATE_XMIT) {
1986
1987 skb = chan->tx_send_head;
1988
1989 bt_cb(skb)->control.retries = 1;
1990 control = &bt_cb(skb)->control;
1991
1992 if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
1993 control->final = 1;
1994
1995 control->reqseq = chan->buffer_seq;
1996 chan->last_acked_seq = chan->buffer_seq;
1997 control->txseq = chan->next_tx_seq;
1998
1999 __pack_control(chan, control, skb);
2000
2001 if (chan->fcs == L2CAP_FCS_CRC16) {
2002 u16 fcs = crc16(0, (u8 *) skb->data, skb->len);
2003 put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE));
2004 }
2005
2006
2007
2008
2009 tx_skb = skb_clone(skb, GFP_KERNEL);
2010
2011 if (!tx_skb)
2012 break;
2013
2014 __set_retrans_timer(chan);
2015
2016 chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq);
2017 chan->unacked_frames++;
2018 chan->frames_sent++;
2019 sent++;
2020
2021 if (skb_queue_is_last(&chan->tx_q, skb))
2022 chan->tx_send_head = NULL;
2023 else
2024 chan->tx_send_head = skb_queue_next(&chan->tx_q, skb);
2025
2026 l2cap_do_send(chan, tx_skb);
2027 BT_DBG("Sent txseq %u", control->txseq);
2028 }
2029
2030 BT_DBG("Sent %d, %u unacked, %u in ERTM queue", sent,
2031 chan->unacked_frames, skb_queue_len(&chan->tx_q));
2032
2033 return sent;
2034}
2035
2036static void l2cap_ertm_resend(struct l2cap_chan *chan)
2037{
2038 struct l2cap_ctrl control;
2039 struct sk_buff *skb;
2040 struct sk_buff *tx_skb;
2041 u16 seq;
2042
2043 BT_DBG("chan %p", chan);
2044
2045 if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
2046 return;
2047
2048 if (__chan_is_moving(chan))
2049 return;
2050
2051 while (chan->retrans_list.head != L2CAP_SEQ_LIST_CLEAR) {
2052 seq = l2cap_seq_list_pop(&chan->retrans_list);
2053
2054 skb = l2cap_ertm_seq_in_queue(&chan->tx_q, seq);
2055 if (!skb) {
2056 BT_DBG("Error: Can't retransmit seq %d, frame missing",
2057 seq);
2058 continue;
2059 }
2060
2061 bt_cb(skb)->control.retries++;
2062 control = bt_cb(skb)->control;
2063
2064 if (chan->max_tx != 0 &&
2065 bt_cb(skb)->control.retries > chan->max_tx) {
2066 BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
2067 l2cap_send_disconn_req(chan, ECONNRESET);
2068 l2cap_seq_list_clear(&chan->retrans_list);
2069 break;
2070 }
2071
2072 control.reqseq = chan->buffer_seq;
2073 if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
2074 control.final = 1;
2075 else
2076 control.final = 0;
2077
2078 if (skb_cloned(skb)) {
2079
2080
2081
2082 tx_skb = skb_copy(skb, GFP_KERNEL);
2083 } else {
2084 tx_skb = skb_clone(skb, GFP_KERNEL);
2085 }
2086
2087 if (!tx_skb) {
2088 l2cap_seq_list_clear(&chan->retrans_list);
2089 break;
2090 }
2091
2092
2093 if (test_bit(FLAG_EXT_CTRL, &chan->flags)) {
2094 put_unaligned_le32(__pack_extended_control(&control),
2095 tx_skb->data + L2CAP_HDR_SIZE);
2096 } else {
2097 put_unaligned_le16(__pack_enhanced_control(&control),
2098 tx_skb->data + L2CAP_HDR_SIZE);
2099 }
2100
2101 if (chan->fcs == L2CAP_FCS_CRC16) {
2102 u16 fcs = crc16(0, (u8 *) tx_skb->data, tx_skb->len);
2103 put_unaligned_le16(fcs, skb_put(tx_skb,
2104 L2CAP_FCS_SIZE));
2105 }
2106
2107 l2cap_do_send(chan, tx_skb);
2108
2109 BT_DBG("Resent txseq %d", control.txseq);
2110
2111 chan->last_acked_seq = chan->buffer_seq;
2112 }
2113}
2114
2115static void l2cap_retransmit(struct l2cap_chan *chan,
2116 struct l2cap_ctrl *control)
2117{
2118 BT_DBG("chan %p, control %p", chan, control);
2119
2120 l2cap_seq_list_append(&chan->retrans_list, control->reqseq);
2121 l2cap_ertm_resend(chan);
2122}
2123
2124static void l2cap_retransmit_all(struct l2cap_chan *chan,
2125 struct l2cap_ctrl *control)
2126{
2127 struct sk_buff *skb;
2128
2129 BT_DBG("chan %p, control %p", chan, control);
2130
2131 if (control->poll)
2132 set_bit(CONN_SEND_FBIT, &chan->conn_state);
2133
2134 l2cap_seq_list_clear(&chan->retrans_list);
2135
2136 if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
2137 return;
2138
2139 if (chan->unacked_frames) {
2140 skb_queue_walk(&chan->tx_q, skb) {
2141 if (bt_cb(skb)->control.txseq == control->reqseq ||
2142 skb == chan->tx_send_head)
2143 break;
2144 }
2145
2146 skb_queue_walk_from(&chan->tx_q, skb) {
2147 if (skb == chan->tx_send_head)
2148 break;
2149
2150 l2cap_seq_list_append(&chan->retrans_list,
2151 bt_cb(skb)->control.txseq);
2152 }
2153
2154 l2cap_ertm_resend(chan);
2155 }
2156}
2157
2158static void l2cap_send_ack(struct l2cap_chan *chan)
2159{
2160 struct l2cap_ctrl control;
2161 u16 frames_to_ack = __seq_offset(chan, chan->buffer_seq,
2162 chan->last_acked_seq);
2163 int threshold;
2164
2165 BT_DBG("chan %p last_acked_seq %d buffer_seq %d",
2166 chan, chan->last_acked_seq, chan->buffer_seq);
2167
2168 memset(&control, 0, sizeof(control));
2169 control.sframe = 1;
2170
2171 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state) &&
2172 chan->rx_state == L2CAP_RX_STATE_RECV) {
2173 __clear_ack_timer(chan);
2174 control.super = L2CAP_SUPER_RNR;
2175 control.reqseq = chan->buffer_seq;
2176 l2cap_send_sframe(chan, &control);
2177 } else {
2178 if (!test_bit(CONN_REMOTE_BUSY, &chan->conn_state)) {
2179 l2cap_ertm_send(chan);
2180
2181 if (chan->buffer_seq == chan->last_acked_seq)
2182 frames_to_ack = 0;
2183 }
2184
2185
2186
2187
2188 threshold = chan->ack_win;
2189 threshold += threshold << 1;
2190 threshold >>= 2;
2191
2192 BT_DBG("frames_to_ack %u, threshold %d", frames_to_ack,
2193 threshold);
2194
2195 if (frames_to_ack >= threshold) {
2196 __clear_ack_timer(chan);
2197 control.super = L2CAP_SUPER_RR;
2198 control.reqseq = chan->buffer_seq;
2199 l2cap_send_sframe(chan, &control);
2200 frames_to_ack = 0;
2201 }
2202
2203 if (frames_to_ack)
2204 __set_ack_timer(chan);
2205 }
2206}
2207
2208static inline int l2cap_skbuff_fromiovec(struct l2cap_chan *chan,
2209 struct msghdr *msg, int len,
2210 int count, struct sk_buff *skb)
2211{
2212 struct l2cap_conn *conn = chan->conn;
2213 struct sk_buff **frag;
2214 int sent = 0;
2215
2216 if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count))
2217 return -EFAULT;
2218
2219 sent += count;
2220 len -= count;
2221
2222
2223 frag = &skb_shinfo(skb)->frag_list;
2224 while (len) {
2225 struct sk_buff *tmp;
2226
2227 count = min_t(unsigned int, conn->mtu, len);
2228
2229 tmp = chan->ops->alloc_skb(chan, count,
2230 msg->msg_flags & MSG_DONTWAIT);
2231 if (IS_ERR(tmp))
2232 return PTR_ERR(tmp);
2233
2234 *frag = tmp;
2235
2236 if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count))
2237 return -EFAULT;
2238
2239 (*frag)->priority = skb->priority;
2240
2241 sent += count;
2242 len -= count;
2243
2244 skb->len += (*frag)->len;
2245 skb->data_len += (*frag)->len;
2246
2247 frag = &(*frag)->next;
2248 }
2249
2250 return sent;
2251}
2252
2253static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan,
2254 struct msghdr *msg, size_t len,
2255 u32 priority)
2256{
2257 struct l2cap_conn *conn = chan->conn;
2258 struct sk_buff *skb;
2259 int err, count, hlen = L2CAP_HDR_SIZE + L2CAP_PSMLEN_SIZE;
2260 struct l2cap_hdr *lh;
2261
2262 BT_DBG("chan %p len %zu priority %u", chan, len, priority);
2263
2264 count = min_t(unsigned int, (conn->mtu - hlen), len);
2265
2266 skb = chan->ops->alloc_skb(chan, count + hlen,
2267 msg->msg_flags & MSG_DONTWAIT);
2268 if (IS_ERR(skb))
2269 return skb;
2270
2271 skb->priority = priority;
2272
2273
2274 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
2275 lh->cid = cpu_to_le16(chan->dcid);
2276 lh->len = cpu_to_le16(len + L2CAP_PSMLEN_SIZE);
2277 put_unaligned(chan->psm, skb_put(skb, L2CAP_PSMLEN_SIZE));
2278
2279 err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb);
2280 if (unlikely(err < 0)) {
2281 kfree_skb(skb);
2282 return ERR_PTR(err);
2283 }
2284 return skb;
2285}
2286
2287static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan,
2288 struct msghdr *msg, size_t len,
2289 u32 priority)
2290{
2291 struct l2cap_conn *conn = chan->conn;
2292 struct sk_buff *skb;
2293 int err, count;
2294 struct l2cap_hdr *lh;
2295
2296 BT_DBG("chan %p len %zu", chan, len);
2297
2298 count = min_t(unsigned int, (conn->mtu - L2CAP_HDR_SIZE), len);
2299
2300 skb = chan->ops->alloc_skb(chan, count + L2CAP_HDR_SIZE,
2301 msg->msg_flags & MSG_DONTWAIT);
2302 if (IS_ERR(skb))
2303 return skb;
2304
2305 skb->priority = priority;
2306
2307
2308 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
2309 lh->cid = cpu_to_le16(chan->dcid);
2310 lh->len = cpu_to_le16(len);
2311
2312 err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb);
2313 if (unlikely(err < 0)) {
2314 kfree_skb(skb);
2315 return ERR_PTR(err);
2316 }
2317 return skb;
2318}
2319
2320static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan,
2321 struct msghdr *msg, size_t len,
2322 u16 sdulen)
2323{
2324 struct l2cap_conn *conn = chan->conn;
2325 struct sk_buff *skb;
2326 int err, count, hlen;
2327 struct l2cap_hdr *lh;
2328
2329 BT_DBG("chan %p len %zu", chan, len);
2330
2331 if (!conn)
2332 return ERR_PTR(-ENOTCONN);
2333
2334 hlen = __ertm_hdr_size(chan);
2335
2336 if (sdulen)
2337 hlen += L2CAP_SDULEN_SIZE;
2338
2339 if (chan->fcs == L2CAP_FCS_CRC16)
2340 hlen += L2CAP_FCS_SIZE;
2341
2342 count = min_t(unsigned int, (conn->mtu - hlen), len);
2343
2344 skb = chan->ops->alloc_skb(chan, count + hlen,
2345 msg->msg_flags & MSG_DONTWAIT);
2346 if (IS_ERR(skb))
2347 return skb;
2348
2349
2350 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
2351 lh->cid = cpu_to_le16(chan->dcid);
2352 lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
2353
2354
2355 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
2356 put_unaligned_le32(0, skb_put(skb, L2CAP_EXT_CTRL_SIZE));
2357 else
2358 put_unaligned_le16(0, skb_put(skb, L2CAP_ENH_CTRL_SIZE));
2359
2360 if (sdulen)
2361 put_unaligned_le16(sdulen, skb_put(skb, L2CAP_SDULEN_SIZE));
2362
2363 err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb);
2364 if (unlikely(err < 0)) {
2365 kfree_skb(skb);
2366 return ERR_PTR(err);
2367 }
2368
2369 bt_cb(skb)->control.fcs = chan->fcs;
2370 bt_cb(skb)->control.retries = 0;
2371 return skb;
2372}
2373
2374static int l2cap_segment_sdu(struct l2cap_chan *chan,
2375 struct sk_buff_head *seg_queue,
2376 struct msghdr *msg, size_t len)
2377{
2378 struct sk_buff *skb;
2379 u16 sdu_len;
2380 size_t pdu_len;
2381 u8 sar;
2382
2383 BT_DBG("chan %p, msg %p, len %zu", chan, msg, len);
2384
2385
2386
2387
2388
2389
2390
2391 pdu_len = chan->conn->mtu;
2392
2393
2394 if (!chan->hs_hcon)
2395 pdu_len = min_t(size_t, pdu_len, L2CAP_BREDR_MAX_PAYLOAD);
2396
2397
2398 if (chan->fcs)
2399 pdu_len -= L2CAP_FCS_SIZE;
2400
2401 pdu_len -= __ertm_hdr_size(chan);
2402
2403
2404 pdu_len = min_t(size_t, pdu_len, chan->remote_mps);
2405
2406 if (len <= pdu_len) {
2407 sar = L2CAP_SAR_UNSEGMENTED;
2408 sdu_len = 0;
2409 pdu_len = len;
2410 } else {
2411 sar = L2CAP_SAR_START;
2412 sdu_len = len;
2413 pdu_len -= L2CAP_SDULEN_SIZE;
2414 }
2415
2416 while (len > 0) {
2417 skb = l2cap_create_iframe_pdu(chan, msg, pdu_len, sdu_len);
2418
2419 if (IS_ERR(skb)) {
2420 __skb_queue_purge(seg_queue);
2421 return PTR_ERR(skb);
2422 }
2423
2424 bt_cb(skb)->control.sar = sar;
2425 __skb_queue_tail(seg_queue, skb);
2426
2427 len -= pdu_len;
2428 if (sdu_len) {
2429 sdu_len = 0;
2430 pdu_len += L2CAP_SDULEN_SIZE;
2431 }
2432
2433 if (len <= pdu_len) {
2434 sar = L2CAP_SAR_END;
2435 pdu_len = len;
2436 } else {
2437 sar = L2CAP_SAR_CONTINUE;
2438 }
2439 }
2440
2441 return 0;
2442}
2443
2444int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len,
2445 u32 priority)
2446{
2447 struct sk_buff *skb;
2448 int err;
2449 struct sk_buff_head seg_queue;
2450
2451
2452 if (chan->chan_type == L2CAP_CHAN_CONN_LESS) {
2453 skb = l2cap_create_connless_pdu(chan, msg, len, priority);
2454 if (IS_ERR(skb))
2455 return PTR_ERR(skb);
2456
2457 l2cap_do_send(chan, skb);
2458 return len;
2459 }
2460
2461 switch (chan->mode) {
2462 case L2CAP_MODE_BASIC:
2463
2464 if (len > chan->omtu)
2465 return -EMSGSIZE;
2466
2467
2468 skb = l2cap_create_basic_pdu(chan, msg, len, priority);
2469 if (IS_ERR(skb))
2470 return PTR_ERR(skb);
2471
2472 l2cap_do_send(chan, skb);
2473 err = len;
2474 break;
2475
2476 case L2CAP_MODE_ERTM:
2477 case L2CAP_MODE_STREAMING:
2478
2479 if (len > chan->omtu) {
2480 err = -EMSGSIZE;
2481 break;
2482 }
2483
2484 __skb_queue_head_init(&seg_queue);
2485
2486
2487
2488
2489
2490 err = l2cap_segment_sdu(chan, &seg_queue, msg, len);
2491
2492
2493
2494
2495 if (chan->state != BT_CONNECTED) {
2496 __skb_queue_purge(&seg_queue);
2497 err = -ENOTCONN;
2498 }
2499
2500 if (err)
2501 break;
2502
2503 if (chan->mode == L2CAP_MODE_ERTM)
2504 l2cap_tx(chan, NULL, &seg_queue, L2CAP_EV_DATA_REQUEST);
2505 else
2506 l2cap_streaming_send(chan, &seg_queue);
2507
2508 err = len;
2509
2510
2511
2512
2513 __skb_queue_purge(&seg_queue);
2514 break;
2515
2516 default:
2517 BT_DBG("bad state %1.1x", chan->mode);
2518 err = -EBADFD;
2519 }
2520
2521 return err;
2522}
2523
2524static void l2cap_send_srej(struct l2cap_chan *chan, u16 txseq)
2525{
2526 struct l2cap_ctrl control;
2527 u16 seq;
2528
2529 BT_DBG("chan %p, txseq %u", chan, txseq);
2530
2531 memset(&control, 0, sizeof(control));
2532 control.sframe = 1;
2533 control.super = L2CAP_SUPER_SREJ;
2534
2535 for (seq = chan->expected_tx_seq; seq != txseq;
2536 seq = __next_seq(chan, seq)) {
2537 if (!l2cap_ertm_seq_in_queue(&chan->srej_q, seq)) {
2538 control.reqseq = seq;
2539 l2cap_send_sframe(chan, &control);
2540 l2cap_seq_list_append(&chan->srej_list, seq);
2541 }
2542 }
2543
2544 chan->expected_tx_seq = __next_seq(chan, txseq);
2545}
2546
2547static void l2cap_send_srej_tail(struct l2cap_chan *chan)
2548{
2549 struct l2cap_ctrl control;
2550
2551 BT_DBG("chan %p", chan);
2552
2553 if (chan->srej_list.tail == L2CAP_SEQ_LIST_CLEAR)
2554 return;
2555
2556 memset(&control, 0, sizeof(control));
2557 control.sframe = 1;
2558 control.super = L2CAP_SUPER_SREJ;
2559 control.reqseq = chan->srej_list.tail;
2560 l2cap_send_sframe(chan, &control);
2561}
2562
2563static void l2cap_send_srej_list(struct l2cap_chan *chan, u16 txseq)
2564{
2565 struct l2cap_ctrl control;
2566 u16 initial_head;
2567 u16 seq;
2568
2569 BT_DBG("chan %p, txseq %u", chan, txseq);
2570
2571 memset(&control, 0, sizeof(control));
2572 control.sframe = 1;
2573 control.super = L2CAP_SUPER_SREJ;
2574
2575
2576 initial_head = chan->srej_list.head;
2577
2578 do {
2579 seq = l2cap_seq_list_pop(&chan->srej_list);
2580 if (seq == txseq || seq == L2CAP_SEQ_LIST_CLEAR)
2581 break;
2582
2583 control.reqseq = seq;
2584 l2cap_send_sframe(chan, &control);
2585 l2cap_seq_list_append(&chan->srej_list, seq);
2586 } while (chan->srej_list.head != initial_head);
2587}
2588
2589static void l2cap_process_reqseq(struct l2cap_chan *chan, u16 reqseq)
2590{
2591 struct sk_buff *acked_skb;
2592 u16 ackseq;
2593
2594 BT_DBG("chan %p, reqseq %u", chan, reqseq);
2595
2596 if (chan->unacked_frames == 0 || reqseq == chan->expected_ack_seq)
2597 return;
2598
2599 BT_DBG("expected_ack_seq %u, unacked_frames %u",
2600 chan->expected_ack_seq, chan->unacked_frames);
2601
2602 for (ackseq = chan->expected_ack_seq; ackseq != reqseq;
2603 ackseq = __next_seq(chan, ackseq)) {
2604
2605 acked_skb = l2cap_ertm_seq_in_queue(&chan->tx_q, ackseq);
2606 if (acked_skb) {
2607 skb_unlink(acked_skb, &chan->tx_q);
2608 kfree_skb(acked_skb);
2609 chan->unacked_frames--;
2610 }
2611 }
2612
2613 chan->expected_ack_seq = reqseq;
2614
2615 if (chan->unacked_frames == 0)
2616 __clear_retrans_timer(chan);
2617
2618 BT_DBG("unacked_frames %u", chan->unacked_frames);
2619}
2620
2621static void l2cap_abort_rx_srej_sent(struct l2cap_chan *chan)
2622{
2623 BT_DBG("chan %p", chan);
2624
2625 chan->expected_tx_seq = chan->buffer_seq;
2626 l2cap_seq_list_clear(&chan->srej_list);
2627 skb_queue_purge(&chan->srej_q);
2628 chan->rx_state = L2CAP_RX_STATE_RECV;
2629}
2630
2631static void l2cap_tx_state_xmit(struct l2cap_chan *chan,
2632 struct l2cap_ctrl *control,
2633 struct sk_buff_head *skbs, u8 event)
2634{
2635 BT_DBG("chan %p, control %p, skbs %p, event %d", chan, control, skbs,
2636 event);
2637
2638 switch (event) {
2639 case L2CAP_EV_DATA_REQUEST:
2640 if (chan->tx_send_head == NULL)
2641 chan->tx_send_head = skb_peek(skbs);
2642
2643 skb_queue_splice_tail_init(skbs, &chan->tx_q);
2644 l2cap_ertm_send(chan);
2645 break;
2646 case L2CAP_EV_LOCAL_BUSY_DETECTED:
2647 BT_DBG("Enter LOCAL_BUSY");
2648 set_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2649
2650 if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
2651
2652
2653
2654 l2cap_abort_rx_srej_sent(chan);
2655 }
2656
2657 l2cap_send_ack(chan);
2658
2659 break;
2660 case L2CAP_EV_LOCAL_BUSY_CLEAR:
2661 BT_DBG("Exit LOCAL_BUSY");
2662 clear_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2663
2664 if (test_bit(CONN_RNR_SENT, &chan->conn_state)) {
2665 struct l2cap_ctrl local_control;
2666
2667 memset(&local_control, 0, sizeof(local_control));
2668 local_control.sframe = 1;
2669 local_control.super = L2CAP_SUPER_RR;
2670 local_control.poll = 1;
2671 local_control.reqseq = chan->buffer_seq;
2672 l2cap_send_sframe(chan, &local_control);
2673
2674 chan->retry_count = 1;
2675 __set_monitor_timer(chan);
2676 chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2677 }
2678 break;
2679 case L2CAP_EV_RECV_REQSEQ_AND_FBIT:
2680 l2cap_process_reqseq(chan, control->reqseq);
2681 break;
2682 case L2CAP_EV_EXPLICIT_POLL:
2683 l2cap_send_rr_or_rnr(chan, 1);
2684 chan->retry_count = 1;
2685 __set_monitor_timer(chan);
2686 __clear_ack_timer(chan);
2687 chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2688 break;
2689 case L2CAP_EV_RETRANS_TO:
2690 l2cap_send_rr_or_rnr(chan, 1);
2691 chan->retry_count = 1;
2692 __set_monitor_timer(chan);
2693 chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2694 break;
2695 case L2CAP_EV_RECV_FBIT:
2696
2697 break;
2698 default:
2699 break;
2700 }
2701}
2702
2703static void l2cap_tx_state_wait_f(struct l2cap_chan *chan,
2704 struct l2cap_ctrl *control,
2705 struct sk_buff_head *skbs, u8 event)
2706{
2707 BT_DBG("chan %p, control %p, skbs %p, event %d", chan, control, skbs,
2708 event);
2709
2710 switch (event) {
2711 case L2CAP_EV_DATA_REQUEST:
2712 if (chan->tx_send_head == NULL)
2713 chan->tx_send_head = skb_peek(skbs);
2714
2715 skb_queue_splice_tail_init(skbs, &chan->tx_q);
2716 break;
2717 case L2CAP_EV_LOCAL_BUSY_DETECTED:
2718 BT_DBG("Enter LOCAL_BUSY");
2719 set_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2720
2721 if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
2722
2723
2724
2725 l2cap_abort_rx_srej_sent(chan);
2726 }
2727
2728 l2cap_send_ack(chan);
2729
2730 break;
2731 case L2CAP_EV_LOCAL_BUSY_CLEAR:
2732 BT_DBG("Exit LOCAL_BUSY");
2733 clear_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2734
2735 if (test_bit(CONN_RNR_SENT, &chan->conn_state)) {
2736 struct l2cap_ctrl local_control;
2737 memset(&local_control, 0, sizeof(local_control));
2738 local_control.sframe = 1;
2739 local_control.super = L2CAP_SUPER_RR;
2740 local_control.poll = 1;
2741 local_control.reqseq = chan->buffer_seq;
2742 l2cap_send_sframe(chan, &local_control);
2743
2744 chan->retry_count = 1;
2745 __set_monitor_timer(chan);
2746 chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2747 }
2748 break;
2749 case L2CAP_EV_RECV_REQSEQ_AND_FBIT:
2750 l2cap_process_reqseq(chan, control->reqseq);
2751
2752
2753
2754 case L2CAP_EV_RECV_FBIT:
2755 if (control && control->final) {
2756 __clear_monitor_timer(chan);
2757 if (chan->unacked_frames > 0)
2758 __set_retrans_timer(chan);
2759 chan->retry_count = 0;
2760 chan->tx_state = L2CAP_TX_STATE_XMIT;
2761 BT_DBG("recv fbit tx_state 0x2.2%x", chan->tx_state);
2762 }
2763 break;
2764 case L2CAP_EV_EXPLICIT_POLL:
2765
2766 break;
2767 case L2CAP_EV_MONITOR_TO:
2768 if (chan->max_tx == 0 || chan->retry_count < chan->max_tx) {
2769 l2cap_send_rr_or_rnr(chan, 1);
2770 __set_monitor_timer(chan);
2771 chan->retry_count++;
2772 } else {
2773 l2cap_send_disconn_req(chan, ECONNABORTED);
2774 }
2775 break;
2776 default:
2777 break;
2778 }
2779}
2780
2781static void l2cap_tx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
2782 struct sk_buff_head *skbs, u8 event)
2783{
2784 BT_DBG("chan %p, control %p, skbs %p, event %d, state %d",
2785 chan, control, skbs, event, chan->tx_state);
2786
2787 switch (chan->tx_state) {
2788 case L2CAP_TX_STATE_XMIT:
2789 l2cap_tx_state_xmit(chan, control, skbs, event);
2790 break;
2791 case L2CAP_TX_STATE_WAIT_F:
2792 l2cap_tx_state_wait_f(chan, control, skbs, event);
2793 break;
2794 default:
2795
2796 break;
2797 }
2798}
2799
2800static void l2cap_pass_to_tx(struct l2cap_chan *chan,
2801 struct l2cap_ctrl *control)
2802{
2803 BT_DBG("chan %p, control %p", chan, control);
2804 l2cap_tx(chan, control, NULL, L2CAP_EV_RECV_REQSEQ_AND_FBIT);
2805}
2806
2807static void l2cap_pass_to_tx_fbit(struct l2cap_chan *chan,
2808 struct l2cap_ctrl *control)
2809{
2810 BT_DBG("chan %p, control %p", chan, control);
2811 l2cap_tx(chan, control, NULL, L2CAP_EV_RECV_FBIT);
2812}
2813
2814
2815static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb)
2816{
2817 struct sk_buff *nskb;
2818 struct l2cap_chan *chan;
2819
2820 BT_DBG("conn %p", conn);
2821
2822 mutex_lock(&conn->chan_lock);
2823
2824 list_for_each_entry(chan, &conn->chan_l, list) {
2825 struct sock *sk = chan->sk;
2826 if (chan->chan_type != L2CAP_CHAN_RAW)
2827 continue;
2828
2829
2830 if (skb->sk == sk)
2831 continue;
2832 nskb = skb_clone(skb, GFP_KERNEL);
2833 if (!nskb)
2834 continue;
2835
2836 if (chan->ops->recv(chan, nskb))
2837 kfree_skb(nskb);
2838 }
2839
2840 mutex_unlock(&conn->chan_lock);
2841}
2842
2843
2844static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn, u8 code,
2845 u8 ident, u16 dlen, void *data)
2846{
2847 struct sk_buff *skb, **frag;
2848 struct l2cap_cmd_hdr *cmd;
2849 struct l2cap_hdr *lh;
2850 int len, count;
2851
2852 BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %u",
2853 conn, code, ident, dlen);
2854
2855 if (conn->mtu < L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE)
2856 return NULL;
2857
2858 len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen;
2859 count = min_t(unsigned int, conn->mtu, len);
2860
2861 skb = bt_skb_alloc(count, GFP_KERNEL);
2862 if (!skb)
2863 return NULL;
2864
2865 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
2866 lh->len = cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen);
2867
2868 if (conn->hcon->type == LE_LINK)
2869 lh->cid = __constant_cpu_to_le16(L2CAP_CID_LE_SIGNALING);
2870 else
2871 lh->cid = __constant_cpu_to_le16(L2CAP_CID_SIGNALING);
2872
2873 cmd = (struct l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE);
2874 cmd->code = code;
2875 cmd->ident = ident;
2876 cmd->len = cpu_to_le16(dlen);
2877
2878 if (dlen) {
2879 count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE;
2880 memcpy(skb_put(skb, count), data, count);
2881 data += count;
2882 }
2883
2884 len -= skb->len;
2885
2886
2887 frag = &skb_shinfo(skb)->frag_list;
2888 while (len) {
2889 count = min_t(unsigned int, conn->mtu, len);
2890
2891 *frag = bt_skb_alloc(count, GFP_KERNEL);
2892 if (!*frag)
2893 goto fail;
2894
2895 memcpy(skb_put(*frag, count), data, count);
2896
2897 len -= count;
2898 data += count;
2899
2900 frag = &(*frag)->next;
2901 }
2902
2903 return skb;
2904
2905fail:
2906 kfree_skb(skb);
2907 return NULL;
2908}
2909
2910static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen,
2911 unsigned long *val)
2912{
2913 struct l2cap_conf_opt *opt = *ptr;
2914 int len;
2915
2916 len = L2CAP_CONF_OPT_SIZE + opt->len;
2917 *ptr += len;
2918
2919 *type = opt->type;
2920 *olen = opt->len;
2921
2922 switch (opt->len) {
2923 case 1:
2924 *val = *((u8 *) opt->val);
2925 break;
2926
2927 case 2:
2928 *val = get_unaligned_le16(opt->val);
2929 break;
2930
2931 case 4:
2932 *val = get_unaligned_le32(opt->val);
2933 break;
2934
2935 default:
2936 *val = (unsigned long) opt->val;
2937 break;
2938 }
2939
2940 BT_DBG("type 0x%2.2x len %u val 0x%lx", *type, opt->len, *val);
2941 return len;
2942}
2943
2944static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val)
2945{
2946 struct l2cap_conf_opt *opt = *ptr;
2947
2948 BT_DBG("type 0x%2.2x len %u val 0x%lx", type, len, val);
2949
2950 opt->type = type;
2951 opt->len = len;
2952
2953 switch (len) {
2954 case 1:
2955 *((u8 *) opt->val) = val;
2956 break;
2957
2958 case 2:
2959 put_unaligned_le16(val, opt->val);
2960 break;
2961
2962 case 4:
2963 put_unaligned_le32(val, opt->val);
2964 break;
2965
2966 default:
2967 memcpy(opt->val, (void *) val, len);
2968 break;
2969 }
2970
2971 *ptr += L2CAP_CONF_OPT_SIZE + len;
2972}
2973
2974static void l2cap_add_opt_efs(void **ptr, struct l2cap_chan *chan)
2975{
2976 struct l2cap_conf_efs efs;
2977
2978 switch (chan->mode) {
2979 case L2CAP_MODE_ERTM:
2980 efs.id = chan->local_id;
2981 efs.stype = chan->local_stype;
2982 efs.msdu = cpu_to_le16(chan->local_msdu);
2983 efs.sdu_itime = cpu_to_le32(chan->local_sdu_itime);
2984 efs.acc_lat = __constant_cpu_to_le32(L2CAP_DEFAULT_ACC_LAT);
2985 efs.flush_to = __constant_cpu_to_le32(L2CAP_EFS_DEFAULT_FLUSH_TO);
2986 break;
2987
2988 case L2CAP_MODE_STREAMING:
2989 efs.id = 1;
2990 efs.stype = L2CAP_SERV_BESTEFFORT;
2991 efs.msdu = cpu_to_le16(chan->local_msdu);
2992 efs.sdu_itime = cpu_to_le32(chan->local_sdu_itime);
2993 efs.acc_lat = 0;
2994 efs.flush_to = 0;
2995 break;
2996
2997 default:
2998 return;
2999 }
3000
3001 l2cap_add_conf_opt(ptr, L2CAP_CONF_EFS, sizeof(efs),
3002 (unsigned long) &efs);
3003}
3004
3005static void l2cap_ack_timeout(struct work_struct *work)
3006{
3007 struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
3008 ack_timer.work);
3009 u16 frames_to_ack;
3010
3011 BT_DBG("chan %p", chan);
3012
3013 l2cap_chan_lock(chan);
3014
3015 frames_to_ack = __seq_offset(chan, chan->buffer_seq,
3016 chan->last_acked_seq);
3017
3018 if (frames_to_ack)
3019 l2cap_send_rr_or_rnr(chan, 0);
3020
3021 l2cap_chan_unlock(chan);
3022 l2cap_chan_put(chan);
3023}
3024
3025int l2cap_ertm_init(struct l2cap_chan *chan)
3026{
3027 int err;
3028
3029 chan->next_tx_seq = 0;
3030 chan->expected_tx_seq = 0;
3031 chan->expected_ack_seq = 0;
3032 chan->unacked_frames = 0;
3033 chan->buffer_seq = 0;
3034 chan->frames_sent = 0;
3035 chan->last_acked_seq = 0;
3036 chan->sdu = NULL;
3037 chan->sdu_last_frag = NULL;
3038 chan->sdu_len = 0;
3039
3040 skb_queue_head_init(&chan->tx_q);
3041
3042 chan->local_amp_id = 0;
3043 chan->move_id = 0;
3044 chan->move_state = L2CAP_MOVE_STABLE;
3045 chan->move_role = L2CAP_MOVE_ROLE_NONE;
3046
3047 if (chan->mode != L2CAP_MODE_ERTM)
3048 return 0;
3049
3050 chan->rx_state = L2CAP_RX_STATE_RECV;
3051 chan->tx_state = L2CAP_TX_STATE_XMIT;
3052
3053 INIT_DELAYED_WORK(&chan->retrans_timer, l2cap_retrans_timeout);
3054 INIT_DELAYED_WORK(&chan->monitor_timer, l2cap_monitor_timeout);
3055 INIT_DELAYED_WORK(&chan->ack_timer, l2cap_ack_timeout);
3056
3057 skb_queue_head_init(&chan->srej_q);
3058
3059 err = l2cap_seq_list_init(&chan->srej_list, chan->tx_win);
3060 if (err < 0)
3061 return err;
3062
3063 err = l2cap_seq_list_init(&chan->retrans_list, chan->remote_tx_win);
3064 if (err < 0)
3065 l2cap_seq_list_free(&chan->srej_list);
3066
3067 return err;
3068}
3069
3070static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask)
3071{
3072 switch (mode) {
3073 case L2CAP_MODE_STREAMING:
3074 case L2CAP_MODE_ERTM:
3075 if (l2cap_mode_supported(mode, remote_feat_mask))
3076 return mode;
3077
3078 default:
3079 return L2CAP_MODE_BASIC;
3080 }
3081}
3082
3083static inline bool __l2cap_ews_supported(struct l2cap_chan *chan)
3084{
3085 return enable_hs && chan->conn->feat_mask & L2CAP_FEAT_EXT_WINDOW;
3086}
3087
3088static inline bool __l2cap_efs_supported(struct l2cap_chan *chan)
3089{
3090 return enable_hs && chan->conn->feat_mask & L2CAP_FEAT_EXT_FLOW;
3091}
3092
3093static void __l2cap_set_ertm_timeouts(struct l2cap_chan *chan,
3094 struct l2cap_conf_rfc *rfc)
3095{
3096 if (chan->local_amp_id && chan->hs_hcon) {
3097 u64 ertm_to = chan->hs_hcon->hdev->amp_be_flush_to;
3098
3099
3100
3101
3102
3103
3104
3105
3106
3107
3108
3109
3110
3111
3112 ertm_to = DIV_ROUND_UP_ULL(ertm_to, 1000);
3113
3114
3115
3116
3117
3118 ertm_to = 3 * ertm_to + 500;
3119
3120 if (ertm_to > 0xffff)
3121 ertm_to = 0xffff;
3122
3123 rfc->retrans_timeout = cpu_to_le16((u16) ertm_to);
3124 rfc->monitor_timeout = rfc->retrans_timeout;
3125 } else {
3126 rfc->retrans_timeout = __constant_cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO);
3127 rfc->monitor_timeout = __constant_cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO);
3128 }
3129}
3130
3131static inline void l2cap_txwin_setup(struct l2cap_chan *chan)
3132{
3133 if (chan->tx_win > L2CAP_DEFAULT_TX_WINDOW &&
3134 __l2cap_ews_supported(chan)) {
3135
3136 set_bit(FLAG_EXT_CTRL, &chan->flags);
3137 chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
3138 } else {
3139 chan->tx_win = min_t(u16, chan->tx_win,
3140 L2CAP_DEFAULT_TX_WINDOW);
3141 chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW;
3142 }
3143 chan->ack_win = chan->tx_win;
3144}
3145
3146static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data)
3147{
3148 struct l2cap_conf_req *req = data;
3149 struct l2cap_conf_rfc rfc = { .mode = chan->mode };
3150 void *ptr = req->data;
3151 u16 size;
3152
3153 BT_DBG("chan %p", chan);
3154
3155 if (chan->num_conf_req || chan->num_conf_rsp)
3156 goto done;
3157
3158 switch (chan->mode) {
3159 case L2CAP_MODE_STREAMING:
3160 case L2CAP_MODE_ERTM:
3161 if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state))
3162 break;
3163
3164 if (__l2cap_efs_supported(chan))
3165 set_bit(FLAG_EFS_ENABLE, &chan->flags);
3166
3167
3168 default:
3169 chan->mode = l2cap_select_mode(rfc.mode, chan->conn->feat_mask);
3170 break;
3171 }
3172
3173done:
3174 if (chan->imtu != L2CAP_DEFAULT_MTU)
3175 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu);
3176
3177 switch (chan->mode) {
3178 case L2CAP_MODE_BASIC:
3179 if (!(chan->conn->feat_mask & L2CAP_FEAT_ERTM) &&
3180 !(chan->conn->feat_mask & L2CAP_FEAT_STREAMING))
3181 break;
3182
3183 rfc.mode = L2CAP_MODE_BASIC;
3184 rfc.txwin_size = 0;
3185 rfc.max_transmit = 0;
3186 rfc.retrans_timeout = 0;
3187 rfc.monitor_timeout = 0;
3188 rfc.max_pdu_size = 0;
3189
3190 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3191 (unsigned long) &rfc);
3192 break;
3193
3194 case L2CAP_MODE_ERTM:
3195 rfc.mode = L2CAP_MODE_ERTM;
3196 rfc.max_transmit = chan->max_tx;
3197
3198 __l2cap_set_ertm_timeouts(chan, &rfc);
3199
3200 size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
3201 L2CAP_EXT_HDR_SIZE - L2CAP_SDULEN_SIZE -
3202 L2CAP_FCS_SIZE);
3203 rfc.max_pdu_size = cpu_to_le16(size);
3204
3205 l2cap_txwin_setup(chan);
3206
3207 rfc.txwin_size = min_t(u16, chan->tx_win,
3208 L2CAP_DEFAULT_TX_WINDOW);
3209
3210 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3211 (unsigned long) &rfc);
3212
3213 if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
3214 l2cap_add_opt_efs(&ptr, chan);
3215
3216 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
3217 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
3218 chan->tx_win);
3219
3220 if (chan->conn->feat_mask & L2CAP_FEAT_FCS)
3221 if (chan->fcs == L2CAP_FCS_NONE ||
3222 test_bit(CONF_RECV_NO_FCS, &chan->conf_state)) {
3223 chan->fcs = L2CAP_FCS_NONE;
3224 l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1,
3225 chan->fcs);
3226 }
3227 break;
3228
3229 case L2CAP_MODE_STREAMING:
3230 l2cap_txwin_setup(chan);
3231 rfc.mode = L2CAP_MODE_STREAMING;
3232 rfc.txwin_size = 0;
3233 rfc.max_transmit = 0;
3234 rfc.retrans_timeout = 0;
3235 rfc.monitor_timeout = 0;
3236
3237 size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
3238 L2CAP_EXT_HDR_SIZE - L2CAP_SDULEN_SIZE -
3239 L2CAP_FCS_SIZE);
3240 rfc.max_pdu_size = cpu_to_le16(size);
3241
3242 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3243 (unsigned long) &rfc);
3244
3245 if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
3246 l2cap_add_opt_efs(&ptr, chan);
3247
3248 if (chan->conn->feat_mask & L2CAP_FEAT_FCS)
3249 if (chan->fcs == L2CAP_FCS_NONE ||
3250 test_bit(CONF_RECV_NO_FCS, &chan->conf_state)) {
3251 chan->fcs = L2CAP_FCS_NONE;
3252 l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1,
3253 chan->fcs);
3254 }
3255 break;
3256 }
3257
3258 req->dcid = cpu_to_le16(chan->dcid);
3259 req->flags = __constant_cpu_to_le16(0);
3260
3261 return ptr - data;
3262}
3263
3264static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data)
3265{
3266 struct l2cap_conf_rsp *rsp = data;
3267 void *ptr = rsp->data;
3268 void *req = chan->conf_req;
3269 int len = chan->conf_len;
3270 int type, hint, olen;
3271 unsigned long val;
3272 struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
3273 struct l2cap_conf_efs efs;
3274 u8 remote_efs = 0;
3275 u16 mtu = L2CAP_DEFAULT_MTU;
3276 u16 result = L2CAP_CONF_SUCCESS;
3277 u16 size;
3278
3279 BT_DBG("chan %p", chan);
3280
3281 while (len >= L2CAP_CONF_OPT_SIZE) {
3282 len -= l2cap_get_conf_opt(&req, &type, &olen, &val);
3283
3284 hint = type & L2CAP_CONF_HINT;
3285 type &= L2CAP_CONF_MASK;
3286
3287 switch (type) {
3288 case L2CAP_CONF_MTU:
3289 mtu = val;
3290 break;
3291
3292 case L2CAP_CONF_FLUSH_TO:
3293 chan->flush_to = val;
3294 break;
3295
3296 case L2CAP_CONF_QOS:
3297 break;
3298
3299 case L2CAP_CONF_RFC:
3300 if (olen == sizeof(rfc))
3301 memcpy(&rfc, (void *) val, olen);
3302 break;
3303
3304 case L2CAP_CONF_FCS:
3305 if (val == L2CAP_FCS_NONE)
3306 set_bit(CONF_RECV_NO_FCS, &chan->conf_state);
3307 break;
3308
3309 case L2CAP_CONF_EFS:
3310 remote_efs = 1;
3311 if (olen == sizeof(efs))
3312 memcpy(&efs, (void *) val, olen);
3313 break;
3314
3315 case L2CAP_CONF_EWS:
3316 if (!enable_hs)
3317 return -ECONNREFUSED;
3318
3319 set_bit(FLAG_EXT_CTRL, &chan->flags);
3320 set_bit(CONF_EWS_RECV, &chan->conf_state);
3321 chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
3322 chan->remote_tx_win = val;
3323 break;
3324
3325 default:
3326 if (hint)
3327 break;
3328
3329 result = L2CAP_CONF_UNKNOWN;
3330 *((u8 *) ptr++) = type;
3331 break;
3332 }
3333 }
3334
3335 if (chan->num_conf_rsp || chan->num_conf_req > 1)
3336 goto done;
3337
3338 switch (chan->mode) {
3339 case L2CAP_MODE_STREAMING:
3340 case L2CAP_MODE_ERTM:
3341 if (!test_bit(CONF_STATE2_DEVICE, &chan->conf_state)) {
3342 chan->mode = l2cap_select_mode(rfc.mode,
3343 chan->conn->feat_mask);
3344 break;
3345 }
3346
3347 if (remote_efs) {
3348 if (__l2cap_efs_supported(chan))
3349 set_bit(FLAG_EFS_ENABLE, &chan->flags);
3350 else
3351 return -ECONNREFUSED;
3352 }
3353
3354 if (chan->mode != rfc.mode)
3355 return -ECONNREFUSED;
3356
3357 break;
3358 }
3359
3360done:
3361 if (chan->mode != rfc.mode) {
3362 result = L2CAP_CONF_UNACCEPT;
3363 rfc.mode = chan->mode;
3364
3365 if (chan->num_conf_rsp == 1)
3366 return -ECONNREFUSED;
3367
3368 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3369 (unsigned long) &rfc);
3370 }
3371
3372 if (result == L2CAP_CONF_SUCCESS) {
3373
3374
3375
3376 if (mtu < L2CAP_DEFAULT_MIN_MTU)
3377 result = L2CAP_CONF_UNACCEPT;
3378 else {
3379 chan->omtu = mtu;
3380 set_bit(CONF_MTU_DONE, &chan->conf_state);
3381 }
3382 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->omtu);
3383
3384 if (remote_efs) {
3385 if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
3386 efs.stype != L2CAP_SERV_NOTRAFIC &&
3387 efs.stype != chan->local_stype) {
3388
3389 result = L2CAP_CONF_UNACCEPT;
3390
3391 if (chan->num_conf_req >= 1)
3392 return -ECONNREFUSED;
3393
3394 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
3395 sizeof(efs),
3396 (unsigned long) &efs);
3397 } else {
3398
3399 result = L2CAP_CONF_PENDING;
3400 set_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
3401 }
3402 }
3403
3404 switch (rfc.mode) {
3405 case L2CAP_MODE_BASIC:
3406 chan->fcs = L2CAP_FCS_NONE;
3407 set_bit(CONF_MODE_DONE, &chan->conf_state);
3408 break;
3409
3410 case L2CAP_MODE_ERTM:
3411 if (!test_bit(CONF_EWS_RECV, &chan->conf_state))
3412 chan->remote_tx_win = rfc.txwin_size;
3413 else
3414 rfc.txwin_size = L2CAP_DEFAULT_TX_WINDOW;
3415
3416 chan->remote_max_tx = rfc.max_transmit;
3417
3418 size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
3419 chan->conn->mtu - L2CAP_EXT_HDR_SIZE -
3420 L2CAP_SDULEN_SIZE - L2CAP_FCS_SIZE);
3421 rfc.max_pdu_size = cpu_to_le16(size);
3422 chan->remote_mps = size;
3423
3424 __l2cap_set_ertm_timeouts(chan, &rfc);
3425
3426 set_bit(CONF_MODE_DONE, &chan->conf_state);
3427
3428 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
3429 sizeof(rfc), (unsigned long) &rfc);
3430
3431 if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
3432 chan->remote_id = efs.id;
3433 chan->remote_stype = efs.stype;
3434 chan->remote_msdu = le16_to_cpu(efs.msdu);
3435 chan->remote_flush_to =
3436 le32_to_cpu(efs.flush_to);
3437 chan->remote_acc_lat =
3438 le32_to_cpu(efs.acc_lat);
3439 chan->remote_sdu_itime =
3440 le32_to_cpu(efs.sdu_itime);
3441 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
3442 sizeof(efs),
3443 (unsigned long) &efs);
3444 }
3445 break;
3446
3447 case L2CAP_MODE_STREAMING:
3448 size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
3449 chan->conn->mtu - L2CAP_EXT_HDR_SIZE -
3450 L2CAP_SDULEN_SIZE - L2CAP_FCS_SIZE);
3451 rfc.max_pdu_size = cpu_to_le16(size);
3452 chan->remote_mps = size;
3453
3454 set_bit(CONF_MODE_DONE, &chan->conf_state);
3455
3456 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3457 (unsigned long) &rfc);
3458
3459 break;
3460
3461 default:
3462 result = L2CAP_CONF_UNACCEPT;
3463
3464 memset(&rfc, 0, sizeof(rfc));
3465 rfc.mode = chan->mode;
3466 }
3467
3468 if (result == L2CAP_CONF_SUCCESS)
3469 set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
3470 }
3471 rsp->scid = cpu_to_le16(chan->dcid);
3472 rsp->result = cpu_to_le16(result);
3473 rsp->flags = __constant_cpu_to_le16(0);
3474
3475 return ptr - data;
3476}
3477
3478static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len,
3479 void *data, u16 *result)
3480{
3481 struct l2cap_conf_req *req = data;
3482 void *ptr = req->data;
3483 int type, olen;
3484 unsigned long val;
3485 struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
3486 struct l2cap_conf_efs efs;
3487
3488 BT_DBG("chan %p, rsp %p, len %d, req %p", chan, rsp, len, data);
3489
3490 while (len >= L2CAP_CONF_OPT_SIZE) {
3491 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
3492
3493 switch (type) {
3494 case L2CAP_CONF_MTU:
3495 if (val < L2CAP_DEFAULT_MIN_MTU) {
3496 *result = L2CAP_CONF_UNACCEPT;
3497 chan->imtu = L2CAP_DEFAULT_MIN_MTU;
3498 } else
3499 chan->imtu = val;
3500 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu);
3501 break;
3502
3503 case L2CAP_CONF_FLUSH_TO:
3504 chan->flush_to = val;
3505 l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
3506 2, chan->flush_to);
3507 break;
3508
3509 case L2CAP_CONF_RFC:
3510 if (olen == sizeof(rfc))
3511 memcpy(&rfc, (void *)val, olen);
3512
3513 if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state) &&
3514 rfc.mode != chan->mode)
3515 return -ECONNREFUSED;
3516
3517 chan->fcs = 0;
3518
3519 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
3520 sizeof(rfc), (unsigned long) &rfc);
3521 break;
3522
3523 case L2CAP_CONF_EWS:
3524 chan->ack_win = min_t(u16, val, chan->ack_win);
3525 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
3526 chan->tx_win);
3527 break;
3528
3529 case L2CAP_CONF_EFS:
3530 if (olen == sizeof(efs))
3531 memcpy(&efs, (void *)val, olen);
3532
3533 if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
3534 efs.stype != L2CAP_SERV_NOTRAFIC &&
3535 efs.stype != chan->local_stype)
3536 return -ECONNREFUSED;
3537
3538 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS, sizeof(efs),
3539 (unsigned long) &efs);
3540 break;
3541
3542 case L2CAP_CONF_FCS:
3543 if (*result == L2CAP_CONF_PENDING)
3544 if (val == L2CAP_FCS_NONE)
3545 set_bit(CONF_RECV_NO_FCS,
3546 &chan->conf_state);
3547 break;
3548 }
3549 }
3550
3551 if (chan->mode == L2CAP_MODE_BASIC && chan->mode != rfc.mode)
3552 return -ECONNREFUSED;
3553
3554 chan->mode = rfc.mode;
3555
3556 if (*result == L2CAP_CONF_SUCCESS || *result == L2CAP_CONF_PENDING) {
3557 switch (rfc.mode) {
3558 case L2CAP_MODE_ERTM:
3559 chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
3560 chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
3561 chan->mps = le16_to_cpu(rfc.max_pdu_size);
3562 if (!test_bit(FLAG_EXT_CTRL, &chan->flags))
3563 chan->ack_win = min_t(u16, chan->ack_win,
3564 rfc.txwin_size);
3565
3566 if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
3567 chan->local_msdu = le16_to_cpu(efs.msdu);
3568 chan->local_sdu_itime =
3569 le32_to_cpu(efs.sdu_itime);
3570 chan->local_acc_lat = le32_to_cpu(efs.acc_lat);
3571 chan->local_flush_to =
3572 le32_to_cpu(efs.flush_to);
3573 }
3574 break;
3575
3576 case L2CAP_MODE_STREAMING:
3577 chan->mps = le16_to_cpu(rfc.max_pdu_size);
3578 }
3579 }
3580
3581 req->dcid = cpu_to_le16(chan->dcid);
3582 req->flags = __constant_cpu_to_le16(0);
3583
3584 return ptr - data;
3585}
3586
3587static int l2cap_build_conf_rsp(struct l2cap_chan *chan, void *data,
3588 u16 result, u16 flags)
3589{
3590 struct l2cap_conf_rsp *rsp = data;
3591 void *ptr = rsp->data;
3592
3593 BT_DBG("chan %p", chan);
3594
3595 rsp->scid = cpu_to_le16(chan->dcid);
3596 rsp->result = cpu_to_le16(result);
3597 rsp->flags = cpu_to_le16(flags);
3598
3599 return ptr - data;
3600}
3601
3602void __l2cap_connect_rsp_defer(struct l2cap_chan *chan)
3603{
3604 struct l2cap_conn_rsp rsp;
3605 struct l2cap_conn *conn = chan->conn;
3606 u8 buf[128];
3607 u8 rsp_code;
3608
3609 rsp.scid = cpu_to_le16(chan->dcid);
3610 rsp.dcid = cpu_to_le16(chan->scid);
3611 rsp.result = __constant_cpu_to_le16(L2CAP_CR_SUCCESS);
3612 rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
3613
3614 if (chan->hs_hcon)
3615 rsp_code = L2CAP_CREATE_CHAN_RSP;
3616 else
3617 rsp_code = L2CAP_CONN_RSP;
3618
3619 BT_DBG("chan %p rsp_code %u", chan, rsp_code);
3620
3621 l2cap_send_cmd(conn, chan->ident, rsp_code, sizeof(rsp), &rsp);
3622
3623 if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
3624 return;
3625
3626 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3627 l2cap_build_conf_req(chan, buf), buf);
3628 chan->num_conf_req++;
3629}
3630
3631static void l2cap_conf_rfc_get(struct l2cap_chan *chan, void *rsp, int len)
3632{
3633 int type, olen;
3634 unsigned long val;
3635
3636
3637
3638 u16 txwin_ext = chan->ack_win;
3639 struct l2cap_conf_rfc rfc = {
3640 .mode = chan->mode,
3641 .retrans_timeout = __constant_cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO),
3642 .monitor_timeout = __constant_cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO),
3643 .max_pdu_size = cpu_to_le16(chan->imtu),
3644 .txwin_size = min_t(u16, chan->ack_win, L2CAP_DEFAULT_TX_WINDOW),
3645 };
3646
3647 BT_DBG("chan %p, rsp %p, len %d", chan, rsp, len);
3648
3649 if ((chan->mode != L2CAP_MODE_ERTM) && (chan->mode != L2CAP_MODE_STREAMING))
3650 return;
3651
3652 while (len >= L2CAP_CONF_OPT_SIZE) {
3653 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
3654
3655 switch (type) {
3656 case L2CAP_CONF_RFC:
3657 if (olen == sizeof(rfc))
3658 memcpy(&rfc, (void *)val, olen);
3659 break;
3660 case L2CAP_CONF_EWS:
3661 txwin_ext = val;
3662 break;
3663 }
3664 }
3665
3666 switch (rfc.mode) {
3667 case L2CAP_MODE_ERTM:
3668 chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
3669 chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
3670 chan->mps = le16_to_cpu(rfc.max_pdu_size);
3671 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
3672 chan->ack_win = min_t(u16, chan->ack_win, txwin_ext);
3673 else
3674 chan->ack_win = min_t(u16, chan->ack_win,
3675 rfc.txwin_size);
3676 break;
3677 case L2CAP_MODE_STREAMING:
3678 chan->mps = le16_to_cpu(rfc.max_pdu_size);
3679 }
3680}
3681
3682static inline int l2cap_command_rej(struct l2cap_conn *conn,
3683 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
3684 u8 *data)
3685{
3686 struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data;
3687
3688 if (cmd_len < sizeof(*rej))
3689 return -EPROTO;
3690
3691 if (rej->reason != L2CAP_REJ_NOT_UNDERSTOOD)
3692 return 0;
3693
3694 if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
3695 cmd->ident == conn->info_ident) {
3696 cancel_delayed_work(&conn->info_timer);
3697
3698 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3699 conn->info_ident = 0;
3700
3701 l2cap_conn_start(conn);
3702 }
3703
3704 return 0;
3705}
3706
3707static struct l2cap_chan *l2cap_connect(struct l2cap_conn *conn,
3708 struct l2cap_cmd_hdr *cmd,
3709 u8 *data, u8 rsp_code, u8 amp_id)
3710{
3711 struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
3712 struct l2cap_conn_rsp rsp;
3713 struct l2cap_chan *chan = NULL, *pchan;
3714 struct sock *parent, *sk = NULL;
3715 int result, status = L2CAP_CS_NO_INFO;
3716
3717 u16 dcid = 0, scid = __le16_to_cpu(req->scid);
3718 __le16 psm = req->psm;
3719
3720 BT_DBG("psm 0x%2.2x scid 0x%4.4x", __le16_to_cpu(psm), scid);
3721
3722
3723 pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, conn->src, conn->dst);
3724 if (!pchan) {
3725 result = L2CAP_CR_BAD_PSM;
3726 goto sendresp;
3727 }
3728
3729 parent = pchan->sk;
3730
3731 mutex_lock(&conn->chan_lock);
3732 lock_sock(parent);
3733
3734
3735 if (psm != __constant_cpu_to_le16(L2CAP_PSM_SDP) &&
3736 !hci_conn_check_link_mode(conn->hcon)) {
3737 conn->disc_reason = HCI_ERROR_AUTH_FAILURE;
3738 result = L2CAP_CR_SEC_BLOCK;
3739 goto response;
3740 }
3741
3742 result = L2CAP_CR_NO_MEM;
3743
3744
3745 if (__l2cap_get_chan_by_dcid(conn, scid))
3746 goto response;
3747
3748 chan = pchan->ops->new_connection(pchan);
3749 if (!chan)
3750 goto response;
3751
3752 sk = chan->sk;
3753
3754 hci_conn_hold(conn->hcon);
3755
3756 bacpy(&bt_sk(sk)->src, conn->src);
3757 bacpy(&bt_sk(sk)->dst, conn->dst);
3758 chan->psm = psm;
3759 chan->dcid = scid;
3760 chan->local_amp_id = amp_id;
3761
3762 __l2cap_chan_add(conn, chan);
3763
3764 dcid = chan->scid;
3765
3766 __set_chan_timer(chan, sk->sk_sndtimeo);
3767
3768 chan->ident = cmd->ident;
3769
3770 if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
3771 if (l2cap_chan_check_security(chan)) {
3772 if (test_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags)) {
3773 __l2cap_state_change(chan, BT_CONNECT2);
3774 result = L2CAP_CR_PEND;
3775 status = L2CAP_CS_AUTHOR_PEND;
3776 chan->ops->defer(chan);
3777 } else {
3778
3779
3780
3781
3782 if (amp_id) {
3783 __l2cap_state_change(chan, BT_CONNECT2);
3784 result = L2CAP_CR_PEND;
3785 } else {
3786 __l2cap_state_change(chan, BT_CONFIG);
3787 result = L2CAP_CR_SUCCESS;
3788 }
3789 status = L2CAP_CS_NO_INFO;
3790 }
3791 } else {
3792 __l2cap_state_change(chan, BT_CONNECT2);
3793 result = L2CAP_CR_PEND;
3794 status = L2CAP_CS_AUTHEN_PEND;
3795 }
3796 } else {
3797 __l2cap_state_change(chan, BT_CONNECT2);
3798 result = L2CAP_CR_PEND;
3799 status = L2CAP_CS_NO_INFO;
3800 }
3801
3802response:
3803 release_sock(parent);
3804 mutex_unlock(&conn->chan_lock);
3805
3806sendresp:
3807 rsp.scid = cpu_to_le16(scid);
3808 rsp.dcid = cpu_to_le16(dcid);
3809 rsp.result = cpu_to_le16(result);
3810 rsp.status = cpu_to_le16(status);
3811 l2cap_send_cmd(conn, cmd->ident, rsp_code, sizeof(rsp), &rsp);
3812
3813 if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
3814 struct l2cap_info_req info;
3815 info.type = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
3816
3817 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
3818 conn->info_ident = l2cap_get_ident(conn);
3819
3820 schedule_delayed_work(&conn->info_timer, L2CAP_INFO_TIMEOUT);
3821
3822 l2cap_send_cmd(conn, conn->info_ident, L2CAP_INFO_REQ,
3823 sizeof(info), &info);
3824 }
3825
3826 if (chan && !test_bit(CONF_REQ_SENT, &chan->conf_state) &&
3827 result == L2CAP_CR_SUCCESS) {
3828 u8 buf[128];
3829 set_bit(CONF_REQ_SENT, &chan->conf_state);
3830 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3831 l2cap_build_conf_req(chan, buf), buf);
3832 chan->num_conf_req++;
3833 }
3834
3835 return chan;
3836}
3837
3838static int l2cap_connect_req(struct l2cap_conn *conn,
3839 struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
3840{
3841 struct hci_dev *hdev = conn->hcon->hdev;
3842 struct hci_conn *hcon = conn->hcon;
3843
3844 if (cmd_len < sizeof(struct l2cap_conn_req))
3845 return -EPROTO;
3846
3847 hci_dev_lock(hdev);
3848 if (test_bit(HCI_MGMT, &hdev->dev_flags) &&
3849 !test_and_set_bit(HCI_CONN_MGMT_CONNECTED, &hcon->flags))
3850 mgmt_device_connected(hdev, &hcon->dst, hcon->type,
3851 hcon->dst_type, 0, NULL, 0,
3852 hcon->dev_class);
3853 hci_dev_unlock(hdev);
3854
3855 l2cap_connect(conn, cmd, data, L2CAP_CONN_RSP, 0);
3856 return 0;
3857}
3858
3859static int l2cap_connect_create_rsp(struct l2cap_conn *conn,
3860 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
3861 u8 *data)
3862{
3863 struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
3864 u16 scid, dcid, result, status;
3865 struct l2cap_chan *chan;
3866 u8 req[128];
3867 int err;
3868
3869 if (cmd_len < sizeof(*rsp))
3870 return -EPROTO;
3871
3872 scid = __le16_to_cpu(rsp->scid);
3873 dcid = __le16_to_cpu(rsp->dcid);
3874 result = __le16_to_cpu(rsp->result);
3875 status = __le16_to_cpu(rsp->status);
3876
3877 BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x",
3878 dcid, scid, result, status);
3879
3880 mutex_lock(&conn->chan_lock);
3881
3882 if (scid) {
3883 chan = __l2cap_get_chan_by_scid(conn, scid);
3884 if (!chan) {
3885 err = -EFAULT;
3886 goto unlock;
3887 }
3888 } else {
3889 chan = __l2cap_get_chan_by_ident(conn, cmd->ident);
3890 if (!chan) {
3891 err = -EFAULT;
3892 goto unlock;
3893 }
3894 }
3895
3896 err = 0;
3897
3898 l2cap_chan_lock(chan);
3899
3900 switch (result) {
3901 case L2CAP_CR_SUCCESS:
3902 l2cap_state_change(chan, BT_CONFIG);
3903 chan->ident = 0;
3904 chan->dcid = dcid;
3905 clear_bit(CONF_CONNECT_PEND, &chan->conf_state);
3906
3907 if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
3908 break;
3909
3910 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3911 l2cap_build_conf_req(chan, req), req);
3912 chan->num_conf_req++;
3913 break;
3914
3915 case L2CAP_CR_PEND:
3916 set_bit(CONF_CONNECT_PEND, &chan->conf_state);
3917 break;
3918
3919 default:
3920 l2cap_chan_del(chan, ECONNREFUSED);
3921 break;
3922 }
3923
3924 l2cap_chan_unlock(chan);
3925
3926unlock:
3927 mutex_unlock(&conn->chan_lock);
3928
3929 return err;
3930}
3931
3932static inline void set_default_fcs(struct l2cap_chan *chan)
3933{
3934
3935
3936
3937 if (chan->mode != L2CAP_MODE_ERTM && chan->mode != L2CAP_MODE_STREAMING)
3938 chan->fcs = L2CAP_FCS_NONE;
3939 else if (!test_bit(CONF_RECV_NO_FCS, &chan->conf_state))
3940 chan->fcs = L2CAP_FCS_CRC16;
3941}
3942
3943static void l2cap_send_efs_conf_rsp(struct l2cap_chan *chan, void *data,
3944 u8 ident, u16 flags)
3945{
3946 struct l2cap_conn *conn = chan->conn;
3947
3948 BT_DBG("conn %p chan %p ident %d flags 0x%4.4x", conn, chan, ident,
3949 flags);
3950
3951 clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
3952 set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
3953
3954 l2cap_send_cmd(conn, ident, L2CAP_CONF_RSP,
3955 l2cap_build_conf_rsp(chan, data,
3956 L2CAP_CONF_SUCCESS, flags), data);
3957}
3958
3959static inline int l2cap_config_req(struct l2cap_conn *conn,
3960 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
3961 u8 *data)
3962{
3963 struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
3964 u16 dcid, flags;
3965 u8 rsp[64];
3966 struct l2cap_chan *chan;
3967 int len, err = 0;
3968
3969 if (cmd_len < sizeof(*req))
3970 return -EPROTO;
3971
3972 dcid = __le16_to_cpu(req->dcid);
3973 flags = __le16_to_cpu(req->flags);
3974
3975 BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
3976
3977 chan = l2cap_get_chan_by_scid(conn, dcid);
3978 if (!chan)
3979 return -ENOENT;
3980
3981 if (chan->state != BT_CONFIG && chan->state != BT_CONNECT2) {
3982 struct l2cap_cmd_rej_cid rej;
3983
3984 rej.reason = __constant_cpu_to_le16(L2CAP_REJ_INVALID_CID);
3985 rej.scid = cpu_to_le16(chan->scid);
3986 rej.dcid = cpu_to_le16(chan->dcid);
3987
3988 l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ,
3989 sizeof(rej), &rej);
3990 goto unlock;
3991 }
3992
3993
3994 len = cmd_len - sizeof(*req);
3995 if (chan->conf_len + len > sizeof(chan->conf_req)) {
3996 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
3997 l2cap_build_conf_rsp(chan, rsp,
3998 L2CAP_CONF_REJECT, flags), rsp);
3999 goto unlock;
4000 }
4001
4002
4003 memcpy(chan->conf_req + chan->conf_len, req->data, len);
4004 chan->conf_len += len;
4005
4006 if (flags & L2CAP_CONF_FLAG_CONTINUATION) {
4007
4008 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
4009 l2cap_build_conf_rsp(chan, rsp,
4010 L2CAP_CONF_SUCCESS, flags), rsp);
4011 goto unlock;
4012 }
4013
4014
4015 len = l2cap_parse_conf_req(chan, rsp);
4016 if (len < 0) {
4017 l2cap_send_disconn_req(chan, ECONNRESET);
4018 goto unlock;
4019 }
4020
4021 chan->ident = cmd->ident;
4022 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
4023 chan->num_conf_rsp++;
4024
4025
4026 chan->conf_len = 0;
4027
4028 if (!test_bit(CONF_OUTPUT_DONE, &chan->conf_state))
4029 goto unlock;
4030
4031 if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
4032 set_default_fcs(chan);
4033
4034 if (chan->mode == L2CAP_MODE_ERTM ||
4035 chan->mode == L2CAP_MODE_STREAMING)
4036 err = l2cap_ertm_init(chan);
4037
4038 if (err < 0)
4039 l2cap_send_disconn_req(chan, -err);
4040 else
4041 l2cap_chan_ready(chan);
4042
4043 goto unlock;
4044 }
4045
4046 if (!test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) {
4047 u8 buf[64];
4048 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
4049 l2cap_build_conf_req(chan, buf), buf);
4050 chan->num_conf_req++;
4051 }
4052
4053
4054
4055 if (test_bit(CONF_REM_CONF_PEND, &chan->conf_state) &&
4056 test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
4057
4058
4059
4060
4061 if (!chan->hs_hcon)
4062 l2cap_send_efs_conf_rsp(chan, rsp, cmd->ident, flags);
4063 else
4064 chan->ident = cmd->ident;
4065 }
4066
4067unlock:
4068 l2cap_chan_unlock(chan);
4069 return err;
4070}
4071
4072static inline int l2cap_config_rsp(struct l2cap_conn *conn,
4073 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4074 u8 *data)
4075{
4076 struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
4077 u16 scid, flags, result;
4078 struct l2cap_chan *chan;
4079 int len = cmd_len - sizeof(*rsp);
4080 int err = 0;
4081
4082 if (cmd_len < sizeof(*rsp))
4083 return -EPROTO;
4084
4085 scid = __le16_to_cpu(rsp->scid);
4086 flags = __le16_to_cpu(rsp->flags);
4087 result = __le16_to_cpu(rsp->result);
4088
4089 BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x len %d", scid, flags,
4090 result, len);
4091
4092 chan = l2cap_get_chan_by_scid(conn, scid);
4093 if (!chan)
4094 return 0;
4095
4096 switch (result) {
4097 case L2CAP_CONF_SUCCESS:
4098 l2cap_conf_rfc_get(chan, rsp->data, len);
4099 clear_bit(CONF_REM_CONF_PEND, &chan->conf_state);
4100 break;
4101
4102 case L2CAP_CONF_PENDING:
4103 set_bit(CONF_REM_CONF_PEND, &chan->conf_state);
4104
4105 if (test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
4106 char buf[64];
4107
4108 len = l2cap_parse_conf_rsp(chan, rsp->data, len,
4109 buf, &result);
4110 if (len < 0) {
4111 l2cap_send_disconn_req(chan, ECONNRESET);
4112 goto done;
4113 }
4114
4115 if (!chan->hs_hcon) {
4116 l2cap_send_efs_conf_rsp(chan, buf, cmd->ident,
4117 0);
4118 } else {
4119 if (l2cap_check_efs(chan)) {
4120 amp_create_logical_link(chan);
4121 chan->ident = cmd->ident;
4122 }
4123 }
4124 }
4125 goto done;
4126
4127 case L2CAP_CONF_UNACCEPT:
4128 if (chan->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
4129 char req[64];
4130
4131 if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) {
4132 l2cap_send_disconn_req(chan, ECONNRESET);
4133 goto done;
4134 }
4135
4136
4137 result = L2CAP_CONF_SUCCESS;
4138 len = l2cap_parse_conf_rsp(chan, rsp->data, len,
4139 req, &result);
4140 if (len < 0) {
4141 l2cap_send_disconn_req(chan, ECONNRESET);
4142 goto done;
4143 }
4144
4145 l2cap_send_cmd(conn, l2cap_get_ident(conn),
4146 L2CAP_CONF_REQ, len, req);
4147 chan->num_conf_req++;
4148 if (result != L2CAP_CONF_SUCCESS)
4149 goto done;
4150 break;
4151 }
4152
4153 default:
4154 l2cap_chan_set_err(chan, ECONNRESET);
4155
4156 __set_chan_timer(chan, L2CAP_DISC_REJ_TIMEOUT);
4157 l2cap_send_disconn_req(chan, ECONNRESET);
4158 goto done;
4159 }
4160
4161 if (flags & L2CAP_CONF_FLAG_CONTINUATION)
4162 goto done;
4163
4164 set_bit(CONF_INPUT_DONE, &chan->conf_state);
4165
4166 if (test_bit(CONF_OUTPUT_DONE, &chan->conf_state)) {
4167 set_default_fcs(chan);
4168
4169 if (chan->mode == L2CAP_MODE_ERTM ||
4170 chan->mode == L2CAP_MODE_STREAMING)
4171 err = l2cap_ertm_init(chan);
4172
4173 if (err < 0)
4174 l2cap_send_disconn_req(chan, -err);
4175 else
4176 l2cap_chan_ready(chan);
4177 }
4178
4179done:
4180 l2cap_chan_unlock(chan);
4181 return err;
4182}
4183
4184static inline int l2cap_disconnect_req(struct l2cap_conn *conn,
4185 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4186 u8 *data)
4187{
4188 struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
4189 struct l2cap_disconn_rsp rsp;
4190 u16 dcid, scid;
4191 struct l2cap_chan *chan;
4192 struct sock *sk;
4193
4194 if (cmd_len != sizeof(*req))
4195 return -EPROTO;
4196
4197 scid = __le16_to_cpu(req->scid);
4198 dcid = __le16_to_cpu(req->dcid);
4199
4200 BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
4201
4202 mutex_lock(&conn->chan_lock);
4203
4204 chan = __l2cap_get_chan_by_scid(conn, dcid);
4205 if (!chan) {
4206 mutex_unlock(&conn->chan_lock);
4207 return 0;
4208 }
4209
4210 l2cap_chan_lock(chan);
4211
4212 sk = chan->sk;
4213
4214 rsp.dcid = cpu_to_le16(chan->scid);
4215 rsp.scid = cpu_to_le16(chan->dcid);
4216 l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
4217
4218 lock_sock(sk);
4219 sk->sk_shutdown = SHUTDOWN_MASK;
4220 release_sock(sk);
4221
4222 l2cap_chan_hold(chan);
4223 l2cap_chan_del(chan, ECONNRESET);
4224
4225 l2cap_chan_unlock(chan);
4226
4227 chan->ops->close(chan);
4228 l2cap_chan_put(chan);
4229
4230 mutex_unlock(&conn->chan_lock);
4231
4232 return 0;
4233}
4234
4235static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn,
4236 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4237 u8 *data)
4238{
4239 struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
4240 u16 dcid, scid;
4241 struct l2cap_chan *chan;
4242
4243 if (cmd_len != sizeof(*rsp))
4244 return -EPROTO;
4245
4246 scid = __le16_to_cpu(rsp->scid);
4247 dcid = __le16_to_cpu(rsp->dcid);
4248
4249 BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
4250
4251 mutex_lock(&conn->chan_lock);
4252
4253 chan = __l2cap_get_chan_by_scid(conn, scid);
4254 if (!chan) {
4255 mutex_unlock(&conn->chan_lock);
4256 return 0;
4257 }
4258
4259 l2cap_chan_lock(chan);
4260
4261 l2cap_chan_hold(chan);
4262 l2cap_chan_del(chan, 0);
4263
4264 l2cap_chan_unlock(chan);
4265
4266 chan->ops->close(chan);
4267 l2cap_chan_put(chan);
4268
4269 mutex_unlock(&conn->chan_lock);
4270
4271 return 0;
4272}
4273
4274static inline int l2cap_information_req(struct l2cap_conn *conn,
4275 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4276 u8 *data)
4277{
4278 struct l2cap_info_req *req = (struct l2cap_info_req *) data;
4279 u16 type;
4280
4281 if (cmd_len != sizeof(*req))
4282 return -EPROTO;
4283
4284 type = __le16_to_cpu(req->type);
4285
4286 BT_DBG("type 0x%4.4x", type);
4287
4288 if (type == L2CAP_IT_FEAT_MASK) {
4289 u8 buf[8];
4290 u32 feat_mask = l2cap_feat_mask;
4291 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
4292 rsp->type = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
4293 rsp->result = __constant_cpu_to_le16(L2CAP_IR_SUCCESS);
4294 if (!disable_ertm)
4295 feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
4296 | L2CAP_FEAT_FCS;
4297 if (enable_hs)
4298 feat_mask |= L2CAP_FEAT_EXT_FLOW
4299 | L2CAP_FEAT_EXT_WINDOW;
4300
4301 put_unaligned_le32(feat_mask, rsp->data);
4302 l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf),
4303 buf);
4304 } else if (type == L2CAP_IT_FIXED_CHAN) {
4305 u8 buf[12];
4306 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
4307
4308 if (enable_hs)
4309 l2cap_fixed_chan[0] |= L2CAP_FC_A2MP;
4310 else
4311 l2cap_fixed_chan[0] &= ~L2CAP_FC_A2MP;
4312
4313 rsp->type = __constant_cpu_to_le16(L2CAP_IT_FIXED_CHAN);
4314 rsp->result = __constant_cpu_to_le16(L2CAP_IR_SUCCESS);
4315 memcpy(rsp->data, l2cap_fixed_chan, sizeof(l2cap_fixed_chan));
4316 l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf),
4317 buf);
4318 } else {
4319 struct l2cap_info_rsp rsp;
4320 rsp.type = cpu_to_le16(type);
4321 rsp.result = __constant_cpu_to_le16(L2CAP_IR_NOTSUPP);
4322 l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(rsp),
4323 &rsp);
4324 }
4325
4326 return 0;
4327}
4328
4329static inline int l2cap_information_rsp(struct l2cap_conn *conn,
4330 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4331 u8 *data)
4332{
4333 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
4334 u16 type, result;
4335
4336 if (cmd_len < sizeof(*rsp))
4337 return -EPROTO;
4338
4339 type = __le16_to_cpu(rsp->type);
4340 result = __le16_to_cpu(rsp->result);
4341
4342 BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
4343
4344
4345 if (cmd->ident != conn->info_ident ||
4346 conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)
4347 return 0;
4348
4349 cancel_delayed_work(&conn->info_timer);
4350
4351 if (result != L2CAP_IR_SUCCESS) {
4352 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4353 conn->info_ident = 0;
4354
4355 l2cap_conn_start(conn);
4356
4357 return 0;
4358 }
4359
4360 switch (type) {
4361 case L2CAP_IT_FEAT_MASK:
4362 conn->feat_mask = get_unaligned_le32(rsp->data);
4363
4364 if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
4365 struct l2cap_info_req req;
4366 req.type = __constant_cpu_to_le16(L2CAP_IT_FIXED_CHAN);
4367
4368 conn->info_ident = l2cap_get_ident(conn);
4369
4370 l2cap_send_cmd(conn, conn->info_ident,
4371 L2CAP_INFO_REQ, sizeof(req), &req);
4372 } else {
4373 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4374 conn->info_ident = 0;
4375
4376 l2cap_conn_start(conn);
4377 }
4378 break;
4379
4380 case L2CAP_IT_FIXED_CHAN:
4381 conn->fixed_chan_mask = rsp->data[0];
4382 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4383 conn->info_ident = 0;
4384
4385 l2cap_conn_start(conn);
4386 break;
4387 }
4388
4389 return 0;
4390}
4391
4392static int l2cap_create_channel_req(struct l2cap_conn *conn,
4393 struct l2cap_cmd_hdr *cmd,
4394 u16 cmd_len, void *data)
4395{
4396 struct l2cap_create_chan_req *req = data;
4397 struct l2cap_create_chan_rsp rsp;
4398 struct l2cap_chan *chan;
4399 struct hci_dev *hdev;
4400 u16 psm, scid;
4401
4402 if (cmd_len != sizeof(*req))
4403 return -EPROTO;
4404
4405 if (!enable_hs)
4406 return -EINVAL;
4407
4408 psm = le16_to_cpu(req->psm);
4409 scid = le16_to_cpu(req->scid);
4410
4411 BT_DBG("psm 0x%2.2x, scid 0x%4.4x, amp_id %d", psm, scid, req->amp_id);
4412
4413
4414 if (req->amp_id == HCI_BREDR_ID) {
4415 l2cap_connect(conn, cmd, data, L2CAP_CREATE_CHAN_RSP,
4416 req->amp_id);
4417 return 0;
4418 }
4419
4420
4421 hdev = hci_dev_get(req->amp_id);
4422 if (!hdev)
4423 goto error;
4424
4425 if (hdev->dev_type != HCI_AMP || !test_bit(HCI_UP, &hdev->flags)) {
4426 hci_dev_put(hdev);
4427 goto error;
4428 }
4429
4430 chan = l2cap_connect(conn, cmd, data, L2CAP_CREATE_CHAN_RSP,
4431 req->amp_id);
4432 if (chan) {
4433 struct amp_mgr *mgr = conn->hcon->amp_mgr;
4434 struct hci_conn *hs_hcon;
4435
4436 hs_hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK, conn->dst);
4437 if (!hs_hcon) {
4438 hci_dev_put(hdev);
4439 return -EFAULT;
4440 }
4441
4442 BT_DBG("mgr %p bredr_chan %p hs_hcon %p", mgr, chan, hs_hcon);
4443
4444 mgr->bredr_chan = chan;
4445 chan->hs_hcon = hs_hcon;
4446 chan->fcs = L2CAP_FCS_NONE;
4447 conn->mtu = hdev->block_mtu;
4448 }
4449
4450 hci_dev_put(hdev);
4451
4452 return 0;
4453
4454error:
4455 rsp.dcid = 0;
4456 rsp.scid = cpu_to_le16(scid);
4457 rsp.result = __constant_cpu_to_le16(L2CAP_CR_BAD_AMP);
4458 rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
4459
4460 l2cap_send_cmd(conn, cmd->ident, L2CAP_CREATE_CHAN_RSP,
4461 sizeof(rsp), &rsp);
4462
4463 return -EFAULT;
4464}
4465
4466static void l2cap_send_move_chan_req(struct l2cap_chan *chan, u8 dest_amp_id)
4467{
4468 struct l2cap_move_chan_req req;
4469 u8 ident;
4470
4471 BT_DBG("chan %p, dest_amp_id %d", chan, dest_amp_id);
4472
4473 ident = l2cap_get_ident(chan->conn);
4474 chan->ident = ident;
4475
4476 req.icid = cpu_to_le16(chan->scid);
4477 req.dest_amp_id = dest_amp_id;
4478
4479 l2cap_send_cmd(chan->conn, ident, L2CAP_MOVE_CHAN_REQ, sizeof(req),
4480 &req);
4481
4482 __set_chan_timer(chan, L2CAP_MOVE_TIMEOUT);
4483}
4484
4485static void l2cap_send_move_chan_rsp(struct l2cap_chan *chan, u16 result)
4486{
4487 struct l2cap_move_chan_rsp rsp;
4488
4489 BT_DBG("chan %p, result 0x%4.4x", chan, result);
4490
4491 rsp.icid = cpu_to_le16(chan->dcid);
4492 rsp.result = cpu_to_le16(result);
4493
4494 l2cap_send_cmd(chan->conn, chan->ident, L2CAP_MOVE_CHAN_RSP,
4495 sizeof(rsp), &rsp);
4496}
4497
4498static void l2cap_send_move_chan_cfm(struct l2cap_chan *chan, u16 result)
4499{
4500 struct l2cap_move_chan_cfm cfm;
4501
4502 BT_DBG("chan %p, result 0x%4.4x", chan, result);
4503
4504 chan->ident = l2cap_get_ident(chan->conn);
4505
4506 cfm.icid = cpu_to_le16(chan->scid);
4507 cfm.result = cpu_to_le16(result);
4508
4509 l2cap_send_cmd(chan->conn, chan->ident, L2CAP_MOVE_CHAN_CFM,
4510 sizeof(cfm), &cfm);
4511
4512 __set_chan_timer(chan, L2CAP_MOVE_TIMEOUT);
4513}
4514
4515static void l2cap_send_move_chan_cfm_icid(struct l2cap_conn *conn, u16 icid)
4516{
4517 struct l2cap_move_chan_cfm cfm;
4518
4519 BT_DBG("conn %p, icid 0x%4.4x", conn, icid);
4520
4521 cfm.icid = cpu_to_le16(icid);
4522 cfm.result = __constant_cpu_to_le16(L2CAP_MC_UNCONFIRMED);
4523
4524 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_MOVE_CHAN_CFM,
4525 sizeof(cfm), &cfm);
4526}
4527
4528static void l2cap_send_move_chan_cfm_rsp(struct l2cap_conn *conn, u8 ident,
4529 u16 icid)
4530{
4531 struct l2cap_move_chan_cfm_rsp rsp;
4532
4533 BT_DBG("icid 0x%4.4x", icid);
4534
4535 rsp.icid = cpu_to_le16(icid);
4536 l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM_RSP, sizeof(rsp), &rsp);
4537}
4538
4539static void __release_logical_link(struct l2cap_chan *chan)
4540{
4541 chan->hs_hchan = NULL;
4542 chan->hs_hcon = NULL;
4543
4544
4545}
4546
4547static void l2cap_logical_fail(struct l2cap_chan *chan)
4548{
4549
4550 if (chan->state != BT_CONNECTED) {
4551
4552 l2cap_send_disconn_req(chan, ECONNRESET);
4553 return;
4554 }
4555
4556 switch (chan->move_role) {
4557 case L2CAP_MOVE_ROLE_RESPONDER:
4558 l2cap_move_done(chan);
4559 l2cap_send_move_chan_rsp(chan, L2CAP_MR_NOT_SUPP);
4560 break;
4561 case L2CAP_MOVE_ROLE_INITIATOR:
4562 if (chan->move_state == L2CAP_MOVE_WAIT_LOGICAL_COMP ||
4563 chan->move_state == L2CAP_MOVE_WAIT_LOGICAL_CFM) {
4564
4565
4566
4567 l2cap_move_done(chan);
4568 }
4569
4570
4571
4572
4573 l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
4574 break;
4575 }
4576}
4577
4578static void l2cap_logical_finish_create(struct l2cap_chan *chan,
4579 struct hci_chan *hchan)
4580{
4581 struct l2cap_conf_rsp rsp;
4582
4583 chan->hs_hchan = hchan;
4584 chan->hs_hcon->l2cap_data = chan->conn;
4585
4586 l2cap_send_efs_conf_rsp(chan, &rsp, chan->ident, 0);
4587
4588 if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
4589 int err;
4590
4591 set_default_fcs(chan);
4592
4593 err = l2cap_ertm_init(chan);
4594 if (err < 0)
4595 l2cap_send_disconn_req(chan, -err);
4596 else
4597 l2cap_chan_ready(chan);
4598 }
4599}
4600
4601static void l2cap_logical_finish_move(struct l2cap_chan *chan,
4602 struct hci_chan *hchan)
4603{
4604 chan->hs_hcon = hchan->conn;
4605 chan->hs_hcon->l2cap_data = chan->conn;
4606
4607 BT_DBG("move_state %d", chan->move_state);
4608
4609 switch (chan->move_state) {
4610 case L2CAP_MOVE_WAIT_LOGICAL_COMP:
4611
4612
4613
4614 chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
4615 break;
4616 case L2CAP_MOVE_WAIT_LOGICAL_CFM:
4617 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
4618 chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
4619 } else if (chan->move_role == L2CAP_MOVE_ROLE_INITIATOR) {
4620 chan->move_state = L2CAP_MOVE_WAIT_CONFIRM_RSP;
4621 l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
4622 } else if (chan->move_role == L2CAP_MOVE_ROLE_RESPONDER) {
4623 chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
4624 l2cap_send_move_chan_rsp(chan, L2CAP_MR_SUCCESS);
4625 }
4626 break;
4627 default:
4628
4629 __release_logical_link(chan);
4630
4631 chan->move_state = L2CAP_MOVE_STABLE;
4632 }
4633}
4634
4635
4636void l2cap_logical_cfm(struct l2cap_chan *chan, struct hci_chan *hchan,
4637 u8 status)
4638{
4639 BT_DBG("chan %p, hchan %p, status %d", chan, hchan, status);
4640
4641 if (status) {
4642 l2cap_logical_fail(chan);
4643 __release_logical_link(chan);
4644 return;
4645 }
4646
4647 if (chan->state != BT_CONNECTED) {
4648
4649 if (chan->local_amp_id)
4650 l2cap_logical_finish_create(chan, hchan);
4651 } else {
4652 l2cap_logical_finish_move(chan, hchan);
4653 }
4654}
4655
4656void l2cap_move_start(struct l2cap_chan *chan)
4657{
4658 BT_DBG("chan %p", chan);
4659
4660 if (chan->local_amp_id == HCI_BREDR_ID) {
4661 if (chan->chan_policy != BT_CHANNEL_POLICY_AMP_PREFERRED)
4662 return;
4663 chan->move_role = L2CAP_MOVE_ROLE_INITIATOR;
4664 chan->move_state = L2CAP_MOVE_WAIT_PREPARE;
4665
4666 } else {
4667 chan->move_role = L2CAP_MOVE_ROLE_INITIATOR;
4668 chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
4669 chan->move_id = 0;
4670 l2cap_move_setup(chan);
4671 l2cap_send_move_chan_req(chan, 0);
4672 }
4673}
4674
4675static void l2cap_do_create(struct l2cap_chan *chan, int result,
4676 u8 local_amp_id, u8 remote_amp_id)
4677{
4678 BT_DBG("chan %p state %s %u -> %u", chan, state_to_string(chan->state),
4679 local_amp_id, remote_amp_id);
4680
4681 chan->fcs = L2CAP_FCS_NONE;
4682
4683
4684 if (chan->state == BT_CONNECT) {
4685 if (result == L2CAP_CR_SUCCESS) {
4686 chan->local_amp_id = local_amp_id;
4687 l2cap_send_create_chan_req(chan, remote_amp_id);
4688 } else {
4689
4690 l2cap_send_conn_req(chan);
4691 }
4692
4693 return;
4694 }
4695
4696
4697 if (__l2cap_no_conn_pending(chan)) {
4698 struct l2cap_conn_rsp rsp;
4699 char buf[128];
4700 rsp.scid = cpu_to_le16(chan->dcid);
4701 rsp.dcid = cpu_to_le16(chan->scid);
4702
4703 if (result == L2CAP_CR_SUCCESS) {
4704
4705 rsp.result = __constant_cpu_to_le16(L2CAP_CR_SUCCESS);
4706 rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
4707 } else {
4708
4709 rsp.result = __constant_cpu_to_le16(L2CAP_CR_NO_MEM);
4710 rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
4711 }
4712
4713 l2cap_send_cmd(chan->conn, chan->ident, L2CAP_CREATE_CHAN_RSP,
4714 sizeof(rsp), &rsp);
4715
4716 if (result == L2CAP_CR_SUCCESS) {
4717 __l2cap_state_change(chan, BT_CONFIG);
4718 set_bit(CONF_REQ_SENT, &chan->conf_state);
4719 l2cap_send_cmd(chan->conn, l2cap_get_ident(chan->conn),
4720 L2CAP_CONF_REQ,
4721 l2cap_build_conf_req(chan, buf), buf);
4722 chan->num_conf_req++;
4723 }
4724 }
4725}
4726
4727static void l2cap_do_move_initiate(struct l2cap_chan *chan, u8 local_amp_id,
4728 u8 remote_amp_id)
4729{
4730 l2cap_move_setup(chan);
4731 chan->move_id = local_amp_id;
4732 chan->move_state = L2CAP_MOVE_WAIT_RSP;
4733
4734 l2cap_send_move_chan_req(chan, remote_amp_id);
4735}
4736
4737static void l2cap_do_move_respond(struct l2cap_chan *chan, int result)
4738{
4739 struct hci_chan *hchan = NULL;
4740
4741
4742
4743 if (hchan) {
4744 if (hchan->state == BT_CONNECTED) {
4745
4746 chan->hs_hcon = hchan->conn;
4747 chan->hs_hcon->l2cap_data = chan->conn;
4748 chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
4749 l2cap_send_move_chan_rsp(chan, L2CAP_MR_SUCCESS);
4750
4751 l2cap_logical_cfm(chan, hchan, L2CAP_MR_SUCCESS);
4752 } else {
4753
4754 chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
4755 }
4756 } else {
4757
4758 l2cap_send_move_chan_rsp(chan, L2CAP_MR_NOT_ALLOWED);
4759 }
4760}
4761
4762static void l2cap_do_move_cancel(struct l2cap_chan *chan, int result)
4763{
4764 if (chan->move_role == L2CAP_MOVE_ROLE_RESPONDER) {
4765 u8 rsp_result;
4766 if (result == -EINVAL)
4767 rsp_result = L2CAP_MR_BAD_ID;
4768 else
4769 rsp_result = L2CAP_MR_NOT_ALLOWED;
4770
4771 l2cap_send_move_chan_rsp(chan, rsp_result);
4772 }
4773
4774 chan->move_role = L2CAP_MOVE_ROLE_NONE;
4775 chan->move_state = L2CAP_MOVE_STABLE;
4776
4777
4778 l2cap_ertm_send(chan);
4779}
4780
4781
4782void __l2cap_physical_cfm(struct l2cap_chan *chan, int result)
4783{
4784 u8 local_amp_id = chan->local_amp_id;
4785 u8 remote_amp_id = chan->remote_amp_id;
4786
4787 BT_DBG("chan %p, result %d, local_amp_id %d, remote_amp_id %d",
4788 chan, result, local_amp_id, remote_amp_id);
4789
4790 if (chan->state == BT_DISCONN || chan->state == BT_CLOSED) {
4791 l2cap_chan_unlock(chan);
4792 return;
4793 }
4794
4795 if (chan->state != BT_CONNECTED) {
4796 l2cap_do_create(chan, result, local_amp_id, remote_amp_id);
4797 } else if (result != L2CAP_MR_SUCCESS) {
4798 l2cap_do_move_cancel(chan, result);
4799 } else {
4800 switch (chan->move_role) {
4801 case L2CAP_MOVE_ROLE_INITIATOR:
4802 l2cap_do_move_initiate(chan, local_amp_id,
4803 remote_amp_id);
4804 break;
4805 case L2CAP_MOVE_ROLE_RESPONDER:
4806 l2cap_do_move_respond(chan, result);
4807 break;
4808 default:
4809 l2cap_do_move_cancel(chan, result);
4810 break;
4811 }
4812 }
4813}
4814
4815static inline int l2cap_move_channel_req(struct l2cap_conn *conn,
4816 struct l2cap_cmd_hdr *cmd,
4817 u16 cmd_len, void *data)
4818{
4819 struct l2cap_move_chan_req *req = data;
4820 struct l2cap_move_chan_rsp rsp;
4821 struct l2cap_chan *chan;
4822 u16 icid = 0;
4823 u16 result = L2CAP_MR_NOT_ALLOWED;
4824
4825 if (cmd_len != sizeof(*req))
4826 return -EPROTO;
4827
4828 icid = le16_to_cpu(req->icid);
4829
4830 BT_DBG("icid 0x%4.4x, dest_amp_id %d", icid, req->dest_amp_id);
4831
4832 if (!enable_hs)
4833 return -EINVAL;
4834
4835 chan = l2cap_get_chan_by_dcid(conn, icid);
4836 if (!chan) {
4837 rsp.icid = cpu_to_le16(icid);
4838 rsp.result = __constant_cpu_to_le16(L2CAP_MR_NOT_ALLOWED);
4839 l2cap_send_cmd(conn, cmd->ident, L2CAP_MOVE_CHAN_RSP,
4840 sizeof(rsp), &rsp);
4841 return 0;
4842 }
4843
4844 chan->ident = cmd->ident;
4845
4846 if (chan->scid < L2CAP_CID_DYN_START ||
4847 chan->chan_policy == BT_CHANNEL_POLICY_BREDR_ONLY ||
4848 (chan->mode != L2CAP_MODE_ERTM &&
4849 chan->mode != L2CAP_MODE_STREAMING)) {
4850 result = L2CAP_MR_NOT_ALLOWED;
4851 goto send_move_response;
4852 }
4853
4854 if (chan->local_amp_id == req->dest_amp_id) {
4855 result = L2CAP_MR_SAME_ID;
4856 goto send_move_response;
4857 }
4858
4859 if (req->dest_amp_id) {
4860 struct hci_dev *hdev;
4861 hdev = hci_dev_get(req->dest_amp_id);
4862 if (!hdev || hdev->dev_type != HCI_AMP ||
4863 !test_bit(HCI_UP, &hdev->flags)) {
4864 if (hdev)
4865 hci_dev_put(hdev);
4866
4867 result = L2CAP_MR_BAD_ID;
4868 goto send_move_response;
4869 }
4870 hci_dev_put(hdev);
4871 }
4872
4873
4874
4875
4876
4877 if ((__chan_is_moving(chan) ||
4878 chan->move_role != L2CAP_MOVE_ROLE_NONE) &&
4879 bacmp(conn->src, conn->dst) > 0) {
4880 result = L2CAP_MR_COLLISION;
4881 goto send_move_response;
4882 }
4883
4884 chan->move_role = L2CAP_MOVE_ROLE_RESPONDER;
4885 l2cap_move_setup(chan);
4886 chan->move_id = req->dest_amp_id;
4887 icid = chan->dcid;
4888
4889 if (!req->dest_amp_id) {
4890
4891 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
4892 chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
4893 result = L2CAP_MR_PEND;
4894 } else {
4895 chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
4896 result = L2CAP_MR_SUCCESS;
4897 }
4898 } else {
4899 chan->move_state = L2CAP_MOVE_WAIT_PREPARE;
4900
4901
4902 result = L2CAP_MR_PEND;
4903 }
4904
4905send_move_response:
4906 l2cap_send_move_chan_rsp(chan, result);
4907
4908 l2cap_chan_unlock(chan);
4909
4910 return 0;
4911}
4912
4913static void l2cap_move_continue(struct l2cap_conn *conn, u16 icid, u16 result)
4914{
4915 struct l2cap_chan *chan;
4916 struct hci_chan *hchan = NULL;
4917
4918 chan = l2cap_get_chan_by_scid(conn, icid);
4919 if (!chan) {
4920 l2cap_send_move_chan_cfm_icid(conn, icid);
4921 return;
4922 }
4923
4924 __clear_chan_timer(chan);
4925 if (result == L2CAP_MR_PEND)
4926 __set_chan_timer(chan, L2CAP_MOVE_ERTX_TIMEOUT);
4927
4928 switch (chan->move_state) {
4929 case L2CAP_MOVE_WAIT_LOGICAL_COMP:
4930
4931
4932
4933 chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
4934 break;
4935 case L2CAP_MOVE_WAIT_RSP_SUCCESS:
4936 if (result == L2CAP_MR_PEND) {
4937 break;
4938 } else if (test_bit(CONN_LOCAL_BUSY,
4939 &chan->conn_state)) {
4940 chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
4941 } else {
4942
4943
4944
4945 chan->move_state = L2CAP_MOVE_WAIT_CONFIRM_RSP;
4946 l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
4947 }
4948 break;
4949 case L2CAP_MOVE_WAIT_RSP:
4950
4951 if (result == L2CAP_MR_SUCCESS) {
4952
4953
4954
4955 chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
4956 } else {
4957
4958
4959
4960 chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_COMP;
4961 }
4962
4963
4964 if (!hchan) {
4965
4966 l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
4967 break;
4968 }
4969
4970
4971
4972
4973 if (hchan->state != BT_CONNECTED)
4974 break;
4975
4976
4977
4978 chan->hs_hcon = hchan->conn;
4979 chan->hs_hcon->l2cap_data = chan->conn;
4980
4981 if (result == L2CAP_MR_SUCCESS) {
4982
4983 l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
4984 } else {
4985
4986
4987
4988 chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
4989 }
4990
4991 l2cap_logical_cfm(chan, hchan, L2CAP_MR_SUCCESS);
4992 break;
4993 default:
4994
4995 chan->move_id = chan->local_amp_id;
4996 l2cap_move_done(chan);
4997 l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
4998 }
4999
5000 l2cap_chan_unlock(chan);
5001}
5002
5003static void l2cap_move_fail(struct l2cap_conn *conn, u8 ident, u16 icid,
5004 u16 result)
5005{
5006 struct l2cap_chan *chan;
5007
5008 chan = l2cap_get_chan_by_ident(conn, ident);
5009 if (!chan) {
5010
5011 l2cap_send_move_chan_cfm_icid(conn, icid);
5012 return;
5013 }
5014
5015 __clear_chan_timer(chan);
5016
5017 if (chan->move_role == L2CAP_MOVE_ROLE_INITIATOR) {
5018 if (result == L2CAP_MR_COLLISION) {
5019 chan->move_role = L2CAP_MOVE_ROLE_RESPONDER;
5020 } else {
5021
5022 chan->move_id = chan->local_amp_id;
5023 l2cap_move_done(chan);
5024 }
5025 }
5026
5027 l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
5028
5029 l2cap_chan_unlock(chan);
5030}
5031
5032static int l2cap_move_channel_rsp(struct l2cap_conn *conn,
5033 struct l2cap_cmd_hdr *cmd,
5034 u16 cmd_len, void *data)
5035{
5036 struct l2cap_move_chan_rsp *rsp = data;
5037 u16 icid, result;
5038
5039 if (cmd_len != sizeof(*rsp))
5040 return -EPROTO;
5041
5042 icid = le16_to_cpu(rsp->icid);
5043 result = le16_to_cpu(rsp->result);
5044
5045 BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result);
5046
5047 if (result == L2CAP_MR_SUCCESS || result == L2CAP_MR_PEND)
5048 l2cap_move_continue(conn, icid, result);
5049 else
5050 l2cap_move_fail(conn, cmd->ident, icid, result);
5051
5052 return 0;
5053}
5054
5055static int l2cap_move_channel_confirm(struct l2cap_conn *conn,
5056 struct l2cap_cmd_hdr *cmd,
5057 u16 cmd_len, void *data)
5058{
5059 struct l2cap_move_chan_cfm *cfm = data;
5060 struct l2cap_chan *chan;
5061 u16 icid, result;
5062
5063 if (cmd_len != sizeof(*cfm))
5064 return -EPROTO;
5065
5066 icid = le16_to_cpu(cfm->icid);
5067 result = le16_to_cpu(cfm->result);
5068
5069 BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result);
5070
5071 chan = l2cap_get_chan_by_dcid(conn, icid);
5072 if (!chan) {
5073
5074 l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid);
5075 return 0;
5076 }
5077
5078 if (chan->move_state == L2CAP_MOVE_WAIT_CONFIRM) {
5079 if (result == L2CAP_MC_CONFIRMED) {
5080 chan->local_amp_id = chan->move_id;
5081 if (!chan->local_amp_id)
5082 __release_logical_link(chan);
5083 } else {
5084 chan->move_id = chan->local_amp_id;
5085 }
5086
5087 l2cap_move_done(chan);
5088 }
5089
5090 l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid);
5091
5092 l2cap_chan_unlock(chan);
5093
5094 return 0;
5095}
5096
5097static inline int l2cap_move_channel_confirm_rsp(struct l2cap_conn *conn,
5098 struct l2cap_cmd_hdr *cmd,
5099 u16 cmd_len, void *data)
5100{
5101 struct l2cap_move_chan_cfm_rsp *rsp = data;
5102 struct l2cap_chan *chan;
5103 u16 icid;
5104
5105 if (cmd_len != sizeof(*rsp))
5106 return -EPROTO;
5107
5108 icid = le16_to_cpu(rsp->icid);
5109
5110 BT_DBG("icid 0x%4.4x", icid);
5111
5112 chan = l2cap_get_chan_by_scid(conn, icid);
5113 if (!chan)
5114 return 0;
5115
5116 __clear_chan_timer(chan);
5117
5118 if (chan->move_state == L2CAP_MOVE_WAIT_CONFIRM_RSP) {
5119 chan->local_amp_id = chan->move_id;
5120
5121 if (!chan->local_amp_id && chan->hs_hchan)
5122 __release_logical_link(chan);
5123
5124 l2cap_move_done(chan);
5125 }
5126
5127 l2cap_chan_unlock(chan);
5128
5129 return 0;
5130}
5131
5132static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency,
5133 u16 to_multiplier)
5134{
5135 u16 max_latency;
5136
5137 if (min > max || min < 6 || max > 3200)
5138 return -EINVAL;
5139
5140 if (to_multiplier < 10 || to_multiplier > 3200)
5141 return -EINVAL;
5142
5143 if (max >= to_multiplier * 8)
5144 return -EINVAL;
5145
5146 max_latency = (to_multiplier * 8 / max) - 1;
5147 if (latency > 499 || latency > max_latency)
5148 return -EINVAL;
5149
5150 return 0;
5151}
5152
5153static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
5154 struct l2cap_cmd_hdr *cmd,
5155 u8 *data)
5156{
5157 struct hci_conn *hcon = conn->hcon;
5158 struct l2cap_conn_param_update_req *req;
5159 struct l2cap_conn_param_update_rsp rsp;
5160 u16 min, max, latency, to_multiplier, cmd_len;
5161 int err;
5162
5163 if (!(hcon->link_mode & HCI_LM_MASTER))
5164 return -EINVAL;
5165
5166 cmd_len = __le16_to_cpu(cmd->len);
5167 if (cmd_len != sizeof(struct l2cap_conn_param_update_req))
5168 return -EPROTO;
5169
5170 req = (struct l2cap_conn_param_update_req *) data;
5171 min = __le16_to_cpu(req->min);
5172 max = __le16_to_cpu(req->max);
5173 latency = __le16_to_cpu(req->latency);
5174 to_multiplier = __le16_to_cpu(req->to_multiplier);
5175
5176 BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x",
5177 min, max, latency, to_multiplier);
5178
5179 memset(&rsp, 0, sizeof(rsp));
5180
5181 err = l2cap_check_conn_param(min, max, latency, to_multiplier);
5182 if (err)
5183 rsp.result = __constant_cpu_to_le16(L2CAP_CONN_PARAM_REJECTED);
5184 else
5185 rsp.result = __constant_cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED);
5186
5187 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP,
5188 sizeof(rsp), &rsp);
5189
5190 if (!err)
5191 hci_le_conn_update(hcon, min, max, latency, to_multiplier);
5192
5193 return 0;
5194}
5195
5196static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
5197 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5198 u8 *data)
5199{
5200 int err = 0;
5201
5202 switch (cmd->code) {
5203 case L2CAP_COMMAND_REJ:
5204 l2cap_command_rej(conn, cmd, cmd_len, data);
5205 break;
5206
5207 case L2CAP_CONN_REQ:
5208 err = l2cap_connect_req(conn, cmd, cmd_len, data);
5209 break;
5210
5211 case L2CAP_CONN_RSP:
5212 case L2CAP_CREATE_CHAN_RSP:
5213 err = l2cap_connect_create_rsp(conn, cmd, cmd_len, data);
5214 break;
5215
5216 case L2CAP_CONF_REQ:
5217 err = l2cap_config_req(conn, cmd, cmd_len, data);
5218 break;
5219
5220 case L2CAP_CONF_RSP:
5221 err = l2cap_config_rsp(conn, cmd, cmd_len, data);
5222 break;
5223
5224 case L2CAP_DISCONN_REQ:
5225 err = l2cap_disconnect_req(conn, cmd, cmd_len, data);
5226 break;
5227
5228 case L2CAP_DISCONN_RSP:
5229 err = l2cap_disconnect_rsp(conn, cmd, cmd_len, data);
5230 break;
5231
5232 case L2CAP_ECHO_REQ:
5233 l2cap_send_cmd(conn, cmd->ident, L2CAP_ECHO_RSP, cmd_len, data);
5234 break;
5235
5236 case L2CAP_ECHO_RSP:
5237 break;
5238
5239 case L2CAP_INFO_REQ:
5240 err = l2cap_information_req(conn, cmd, cmd_len, data);
5241 break;
5242
5243 case L2CAP_INFO_RSP:
5244 err = l2cap_information_rsp(conn, cmd, cmd_len, data);
5245 break;
5246
5247 case L2CAP_CREATE_CHAN_REQ:
5248 err = l2cap_create_channel_req(conn, cmd, cmd_len, data);
5249 break;
5250
5251 case L2CAP_MOVE_CHAN_REQ:
5252 err = l2cap_move_channel_req(conn, cmd, cmd_len, data);
5253 break;
5254
5255 case L2CAP_MOVE_CHAN_RSP:
5256 err = l2cap_move_channel_rsp(conn, cmd, cmd_len, data);
5257 break;
5258
5259 case L2CAP_MOVE_CHAN_CFM:
5260 err = l2cap_move_channel_confirm(conn, cmd, cmd_len, data);
5261 break;
5262
5263 case L2CAP_MOVE_CHAN_CFM_RSP:
5264 err = l2cap_move_channel_confirm_rsp(conn, cmd, cmd_len, data);
5265 break;
5266
5267 default:
5268 BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code);
5269 err = -EINVAL;
5270 break;
5271 }
5272
5273 return err;
5274}
5275
5276static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn,
5277 struct l2cap_cmd_hdr *cmd, u8 *data)
5278{
5279 switch (cmd->code) {
5280 case L2CAP_COMMAND_REJ:
5281 return 0;
5282
5283 case L2CAP_CONN_PARAM_UPDATE_REQ:
5284 return l2cap_conn_param_update_req(conn, cmd, data);
5285
5286 case L2CAP_CONN_PARAM_UPDATE_RSP:
5287 return 0;
5288
5289 default:
5290 BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code);
5291 return -EINVAL;
5292 }
5293}
5294
5295static inline void l2cap_sig_channel(struct l2cap_conn *conn,
5296 struct sk_buff *skb)
5297{
5298 u8 *data = skb->data;
5299 int len = skb->len;
5300 struct l2cap_cmd_hdr cmd;
5301 int err;
5302
5303 l2cap_raw_recv(conn, skb);
5304
5305 while (len >= L2CAP_CMD_HDR_SIZE) {
5306 u16 cmd_len;
5307 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
5308 data += L2CAP_CMD_HDR_SIZE;
5309 len -= L2CAP_CMD_HDR_SIZE;
5310
5311 cmd_len = le16_to_cpu(cmd.len);
5312
5313 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len,
5314 cmd.ident);
5315
5316 if (cmd_len > len || !cmd.ident) {
5317 BT_DBG("corrupted command");
5318 break;
5319 }
5320
5321 if (conn->hcon->type == LE_LINK)
5322 err = l2cap_le_sig_cmd(conn, &cmd, data);
5323 else
5324 err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data);
5325
5326 if (err) {
5327 struct l2cap_cmd_rej_unk rej;
5328
5329 BT_ERR("Wrong link type (%d)", err);
5330
5331
5332 rej.reason = __constant_cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
5333 l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ,
5334 sizeof(rej), &rej);
5335 }
5336
5337 data += cmd_len;
5338 len -= cmd_len;
5339 }
5340
5341 kfree_skb(skb);
5342}
5343
5344static int l2cap_check_fcs(struct l2cap_chan *chan, struct sk_buff *skb)
5345{
5346 u16 our_fcs, rcv_fcs;
5347 int hdr_size;
5348
5349 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
5350 hdr_size = L2CAP_EXT_HDR_SIZE;
5351 else
5352 hdr_size = L2CAP_ENH_HDR_SIZE;
5353
5354 if (chan->fcs == L2CAP_FCS_CRC16) {
5355 skb_trim(skb, skb->len - L2CAP_FCS_SIZE);
5356 rcv_fcs = get_unaligned_le16(skb->data + skb->len);
5357 our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
5358
5359 if (our_fcs != rcv_fcs)
5360 return -EBADMSG;
5361 }
5362 return 0;
5363}
5364
5365static void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan)
5366{
5367 struct l2cap_ctrl control;
5368
5369 BT_DBG("chan %p", chan);
5370
5371 memset(&control, 0, sizeof(control));
5372 control.sframe = 1;
5373 control.final = 1;
5374 control.reqseq = chan->buffer_seq;
5375 set_bit(CONN_SEND_FBIT, &chan->conn_state);
5376
5377 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
5378 control.super = L2CAP_SUPER_RNR;
5379 l2cap_send_sframe(chan, &control);
5380 }
5381
5382 if (test_and_clear_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
5383 chan->unacked_frames > 0)
5384 __set_retrans_timer(chan);
5385
5386
5387 l2cap_ertm_send(chan);
5388
5389 if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state) &&
5390 test_bit(CONN_SEND_FBIT, &chan->conn_state)) {
5391
5392
5393
5394 control.super = L2CAP_SUPER_RR;
5395 l2cap_send_sframe(chan, &control);
5396 }
5397}
5398
5399static void append_skb_frag(struct sk_buff *skb, struct sk_buff *new_frag,
5400 struct sk_buff **last_frag)
5401{
5402
5403
5404
5405 if (!skb_has_frag_list(skb))
5406 skb_shinfo(skb)->frag_list = new_frag;
5407
5408 new_frag->next = NULL;
5409
5410 (*last_frag)->next = new_frag;
5411 *last_frag = new_frag;
5412
5413 skb->len += new_frag->len;
5414 skb->data_len += new_frag->len;
5415 skb->truesize += new_frag->truesize;
5416}
5417
5418static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb,
5419 struct l2cap_ctrl *control)
5420{
5421 int err = -EINVAL;
5422
5423 switch (control->sar) {
5424 case L2CAP_SAR_UNSEGMENTED:
5425 if (chan->sdu)
5426 break;
5427
5428 err = chan->ops->recv(chan, skb);
5429 break;
5430
5431 case L2CAP_SAR_START:
5432 if (chan->sdu)
5433 break;
5434
5435 chan->sdu_len = get_unaligned_le16(skb->data);
5436 skb_pull(skb, L2CAP_SDULEN_SIZE);
5437
5438 if (chan->sdu_len > chan->imtu) {
5439 err = -EMSGSIZE;
5440 break;
5441 }
5442
5443 if (skb->len >= chan->sdu_len)
5444 break;
5445
5446 chan->sdu = skb;
5447 chan->sdu_last_frag = skb;
5448
5449 skb = NULL;
5450 err = 0;
5451 break;
5452
5453 case L2CAP_SAR_CONTINUE:
5454 if (!chan->sdu)
5455 break;
5456
5457 append_skb_frag(chan->sdu, skb,
5458 &chan->sdu_last_frag);
5459 skb = NULL;
5460
5461 if (chan->sdu->len >= chan->sdu_len)
5462 break;
5463
5464 err = 0;
5465 break;
5466
5467 case L2CAP_SAR_END:
5468 if (!chan->sdu)
5469 break;
5470
5471 append_skb_frag(chan->sdu, skb,
5472 &chan->sdu_last_frag);
5473 skb = NULL;
5474
5475 if (chan->sdu->len != chan->sdu_len)
5476 break;
5477
5478 err = chan->ops->recv(chan, chan->sdu);
5479
5480 if (!err) {
5481
5482 chan->sdu = NULL;
5483 chan->sdu_last_frag = NULL;
5484 chan->sdu_len = 0;
5485 }
5486 break;
5487 }
5488
5489 if (err) {
5490 kfree_skb(skb);
5491 kfree_skb(chan->sdu);
5492 chan->sdu = NULL;
5493 chan->sdu_last_frag = NULL;
5494 chan->sdu_len = 0;
5495 }
5496
5497 return err;
5498}
5499
5500static int l2cap_resegment(struct l2cap_chan *chan)
5501{
5502
5503 return 0;
5504}
5505
5506void l2cap_chan_busy(struct l2cap_chan *chan, int busy)
5507{
5508 u8 event;
5509
5510 if (chan->mode != L2CAP_MODE_ERTM)
5511 return;
5512
5513 event = busy ? L2CAP_EV_LOCAL_BUSY_DETECTED : L2CAP_EV_LOCAL_BUSY_CLEAR;
5514 l2cap_tx(chan, NULL, NULL, event);
5515}
5516
5517static int l2cap_rx_queued_iframes(struct l2cap_chan *chan)
5518{
5519 int err = 0;
5520
5521
5522
5523
5524 BT_DBG("chan %p", chan);
5525
5526 while (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
5527 struct sk_buff *skb;
5528 BT_DBG("Searching for skb with txseq %d (queue len %d)",
5529 chan->buffer_seq, skb_queue_len(&chan->srej_q));
5530
5531 skb = l2cap_ertm_seq_in_queue(&chan->srej_q, chan->buffer_seq);
5532
5533 if (!skb)
5534 break;
5535
5536 skb_unlink(skb, &chan->srej_q);
5537 chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
5538 err = l2cap_reassemble_sdu(chan, skb, &bt_cb(skb)->control);
5539 if (err)
5540 break;
5541 }
5542
5543 if (skb_queue_empty(&chan->srej_q)) {
5544 chan->rx_state = L2CAP_RX_STATE_RECV;
5545 l2cap_send_ack(chan);
5546 }
5547
5548 return err;
5549}
5550
5551static void l2cap_handle_srej(struct l2cap_chan *chan,
5552 struct l2cap_ctrl *control)
5553{
5554 struct sk_buff *skb;
5555
5556 BT_DBG("chan %p, control %p", chan, control);
5557
5558 if (control->reqseq == chan->next_tx_seq) {
5559 BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq);
5560 l2cap_send_disconn_req(chan, ECONNRESET);
5561 return;
5562 }
5563
5564 skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq);
5565
5566 if (skb == NULL) {
5567 BT_DBG("Seq %d not available for retransmission",
5568 control->reqseq);
5569 return;
5570 }
5571
5572 if (chan->max_tx != 0 && bt_cb(skb)->control.retries >= chan->max_tx) {
5573 BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
5574 l2cap_send_disconn_req(chan, ECONNRESET);
5575 return;
5576 }
5577
5578 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
5579
5580 if (control->poll) {
5581 l2cap_pass_to_tx(chan, control);
5582
5583 set_bit(CONN_SEND_FBIT, &chan->conn_state);
5584 l2cap_retransmit(chan, control);
5585 l2cap_ertm_send(chan);
5586
5587 if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) {
5588 set_bit(CONN_SREJ_ACT, &chan->conn_state);
5589 chan->srej_save_reqseq = control->reqseq;
5590 }
5591 } else {
5592 l2cap_pass_to_tx_fbit(chan, control);
5593
5594 if (control->final) {
5595 if (chan->srej_save_reqseq != control->reqseq ||
5596 !test_and_clear_bit(CONN_SREJ_ACT,
5597 &chan->conn_state))
5598 l2cap_retransmit(chan, control);
5599 } else {
5600 l2cap_retransmit(chan, control);
5601 if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) {
5602 set_bit(CONN_SREJ_ACT, &chan->conn_state);
5603 chan->srej_save_reqseq = control->reqseq;
5604 }
5605 }
5606 }
5607}
5608
5609static void l2cap_handle_rej(struct l2cap_chan *chan,
5610 struct l2cap_ctrl *control)
5611{
5612 struct sk_buff *skb;
5613
5614 BT_DBG("chan %p, control %p", chan, control);
5615
5616 if (control->reqseq == chan->next_tx_seq) {
5617 BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq);
5618 l2cap_send_disconn_req(chan, ECONNRESET);
5619 return;
5620 }
5621
5622 skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq);
5623
5624 if (chan->max_tx && skb &&
5625 bt_cb(skb)->control.retries >= chan->max_tx) {
5626 BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
5627 l2cap_send_disconn_req(chan, ECONNRESET);
5628 return;
5629 }
5630
5631 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
5632
5633 l2cap_pass_to_tx(chan, control);
5634
5635 if (control->final) {
5636 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
5637 l2cap_retransmit_all(chan, control);
5638 } else {
5639 l2cap_retransmit_all(chan, control);
5640 l2cap_ertm_send(chan);
5641 if (chan->tx_state == L2CAP_TX_STATE_WAIT_F)
5642 set_bit(CONN_REJ_ACT, &chan->conn_state);
5643 }
5644}
5645
5646static u8 l2cap_classify_txseq(struct l2cap_chan *chan, u16 txseq)
5647{
5648 BT_DBG("chan %p, txseq %d", chan, txseq);
5649
5650 BT_DBG("last_acked_seq %d, expected_tx_seq %d", chan->last_acked_seq,
5651 chan->expected_tx_seq);
5652
5653 if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
5654 if (__seq_offset(chan, txseq, chan->last_acked_seq) >=
5655 chan->tx_win) {
5656
5657
5658
5659 if (chan->tx_win <= ((chan->tx_win_max + 1) >> 1)) {
5660 BT_DBG("Invalid/Ignore - after SREJ");
5661 return L2CAP_TXSEQ_INVALID_IGNORE;
5662 } else {
5663 BT_DBG("Invalid - in window after SREJ sent");
5664 return L2CAP_TXSEQ_INVALID;
5665 }
5666 }
5667
5668 if (chan->srej_list.head == txseq) {
5669 BT_DBG("Expected SREJ");
5670 return L2CAP_TXSEQ_EXPECTED_SREJ;
5671 }
5672
5673 if (l2cap_ertm_seq_in_queue(&chan->srej_q, txseq)) {
5674 BT_DBG("Duplicate SREJ - txseq already stored");
5675 return L2CAP_TXSEQ_DUPLICATE_SREJ;
5676 }
5677
5678 if (l2cap_seq_list_contains(&chan->srej_list, txseq)) {
5679 BT_DBG("Unexpected SREJ - not requested");
5680 return L2CAP_TXSEQ_UNEXPECTED_SREJ;
5681 }
5682 }
5683
5684 if (chan->expected_tx_seq == txseq) {
5685 if (__seq_offset(chan, txseq, chan->last_acked_seq) >=
5686 chan->tx_win) {
5687 BT_DBG("Invalid - txseq outside tx window");
5688 return L2CAP_TXSEQ_INVALID;
5689 } else {
5690 BT_DBG("Expected");
5691 return L2CAP_TXSEQ_EXPECTED;
5692 }
5693 }
5694
5695 if (__seq_offset(chan, txseq, chan->last_acked_seq) <
5696 __seq_offset(chan, chan->expected_tx_seq, chan->last_acked_seq)) {
5697 BT_DBG("Duplicate - expected_tx_seq later than txseq");
5698 return L2CAP_TXSEQ_DUPLICATE;
5699 }
5700
5701 if (__seq_offset(chan, txseq, chan->last_acked_seq) >= chan->tx_win) {
5702
5703
5704
5705
5706
5707
5708
5709
5710
5711
5712
5713
5714
5715
5716
5717
5718
5719 if (chan->tx_win <= ((chan->tx_win_max + 1) >> 1)) {
5720 BT_DBG("Invalid/Ignore - txseq outside tx window");
5721 return L2CAP_TXSEQ_INVALID_IGNORE;
5722 } else {
5723 BT_DBG("Invalid - txseq outside tx window");
5724 return L2CAP_TXSEQ_INVALID;
5725 }
5726 } else {
5727 BT_DBG("Unexpected - txseq indicates missing frames");
5728 return L2CAP_TXSEQ_UNEXPECTED;
5729 }
5730}
5731
5732static int l2cap_rx_state_recv(struct l2cap_chan *chan,
5733 struct l2cap_ctrl *control,
5734 struct sk_buff *skb, u8 event)
5735{
5736 int err = 0;
5737 bool skb_in_use = 0;
5738
5739 BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
5740 event);
5741
5742 switch (event) {
5743 case L2CAP_EV_RECV_IFRAME:
5744 switch (l2cap_classify_txseq(chan, control->txseq)) {
5745 case L2CAP_TXSEQ_EXPECTED:
5746 l2cap_pass_to_tx(chan, control);
5747
5748 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
5749 BT_DBG("Busy, discarding expected seq %d",
5750 control->txseq);
5751 break;
5752 }
5753
5754 chan->expected_tx_seq = __next_seq(chan,
5755 control->txseq);
5756
5757 chan->buffer_seq = chan->expected_tx_seq;
5758 skb_in_use = 1;
5759
5760 err = l2cap_reassemble_sdu(chan, skb, control);
5761 if (err)
5762 break;
5763
5764 if (control->final) {
5765 if (!test_and_clear_bit(CONN_REJ_ACT,
5766 &chan->conn_state)) {
5767 control->final = 0;
5768 l2cap_retransmit_all(chan, control);
5769 l2cap_ertm_send(chan);
5770 }
5771 }
5772
5773 if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state))
5774 l2cap_send_ack(chan);
5775 break;
5776 case L2CAP_TXSEQ_UNEXPECTED:
5777 l2cap_pass_to_tx(chan, control);
5778
5779
5780
5781
5782
5783 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
5784 BT_DBG("Busy, discarding unexpected seq %d",
5785 control->txseq);
5786 break;
5787 }
5788
5789
5790
5791
5792
5793 skb_queue_tail(&chan->srej_q, skb);
5794 skb_in_use = 1;
5795 BT_DBG("Queued %p (queue len %d)", skb,
5796 skb_queue_len(&chan->srej_q));
5797
5798 clear_bit(CONN_SREJ_ACT, &chan->conn_state);
5799 l2cap_seq_list_clear(&chan->srej_list);
5800 l2cap_send_srej(chan, control->txseq);
5801
5802 chan->rx_state = L2CAP_RX_STATE_SREJ_SENT;
5803 break;
5804 case L2CAP_TXSEQ_DUPLICATE:
5805 l2cap_pass_to_tx(chan, control);
5806 break;
5807 case L2CAP_TXSEQ_INVALID_IGNORE:
5808 break;
5809 case L2CAP_TXSEQ_INVALID:
5810 default:
5811 l2cap_send_disconn_req(chan, ECONNRESET);
5812 break;
5813 }
5814 break;
5815 case L2CAP_EV_RECV_RR:
5816 l2cap_pass_to_tx(chan, control);
5817 if (control->final) {
5818 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
5819
5820 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state) &&
5821 !__chan_is_moving(chan)) {
5822 control->final = 0;
5823 l2cap_retransmit_all(chan, control);
5824 }
5825
5826 l2cap_ertm_send(chan);
5827 } else if (control->poll) {
5828 l2cap_send_i_or_rr_or_rnr(chan);
5829 } else {
5830 if (test_and_clear_bit(CONN_REMOTE_BUSY,
5831 &chan->conn_state) &&
5832 chan->unacked_frames)
5833 __set_retrans_timer(chan);
5834
5835 l2cap_ertm_send(chan);
5836 }
5837 break;
5838 case L2CAP_EV_RECV_RNR:
5839 set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
5840 l2cap_pass_to_tx(chan, control);
5841 if (control && control->poll) {
5842 set_bit(CONN_SEND_FBIT, &chan->conn_state);
5843 l2cap_send_rr_or_rnr(chan, 0);
5844 }
5845 __clear_retrans_timer(chan);
5846 l2cap_seq_list_clear(&chan->retrans_list);
5847 break;
5848 case L2CAP_EV_RECV_REJ:
5849 l2cap_handle_rej(chan, control);
5850 break;
5851 case L2CAP_EV_RECV_SREJ:
5852 l2cap_handle_srej(chan, control);
5853 break;
5854 default:
5855 break;
5856 }
5857
5858 if (skb && !skb_in_use) {
5859 BT_DBG("Freeing %p", skb);
5860 kfree_skb(skb);
5861 }
5862
5863 return err;
5864}
5865
5866static int l2cap_rx_state_srej_sent(struct l2cap_chan *chan,
5867 struct l2cap_ctrl *control,
5868 struct sk_buff *skb, u8 event)
5869{
5870 int err = 0;
5871 u16 txseq = control->txseq;
5872 bool skb_in_use = 0;
5873
5874 BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
5875 event);
5876
5877 switch (event) {
5878 case L2CAP_EV_RECV_IFRAME:
5879 switch (l2cap_classify_txseq(chan, txseq)) {
5880 case L2CAP_TXSEQ_EXPECTED:
5881
5882 l2cap_pass_to_tx(chan, control);
5883 skb_queue_tail(&chan->srej_q, skb);
5884 skb_in_use = 1;
5885 BT_DBG("Queued %p (queue len %d)", skb,
5886 skb_queue_len(&chan->srej_q));
5887
5888 chan->expected_tx_seq = __next_seq(chan, txseq);
5889 break;
5890 case L2CAP_TXSEQ_EXPECTED_SREJ:
5891 l2cap_seq_list_pop(&chan->srej_list);
5892
5893 l2cap_pass_to_tx(chan, control);
5894 skb_queue_tail(&chan->srej_q, skb);
5895 skb_in_use = 1;
5896 BT_DBG("Queued %p (queue len %d)", skb,
5897 skb_queue_len(&chan->srej_q));
5898
5899 err = l2cap_rx_queued_iframes(chan);
5900 if (err)
5901 break;
5902
5903 break;
5904 case L2CAP_TXSEQ_UNEXPECTED:
5905
5906
5907
5908
5909 skb_queue_tail(&chan->srej_q, skb);
5910 skb_in_use = 1;
5911 BT_DBG("Queued %p (queue len %d)", skb,
5912 skb_queue_len(&chan->srej_q));
5913
5914 l2cap_pass_to_tx(chan, control);
5915 l2cap_send_srej(chan, control->txseq);
5916 break;
5917 case L2CAP_TXSEQ_UNEXPECTED_SREJ:
5918
5919
5920
5921
5922
5923 skb_queue_tail(&chan->srej_q, skb);
5924 skb_in_use = 1;
5925 BT_DBG("Queued %p (queue len %d)", skb,
5926 skb_queue_len(&chan->srej_q));
5927
5928 l2cap_pass_to_tx(chan, control);
5929 l2cap_send_srej_list(chan, control->txseq);
5930 break;
5931 case L2CAP_TXSEQ_DUPLICATE_SREJ:
5932
5933 l2cap_pass_to_tx(chan, control);
5934 break;
5935 case L2CAP_TXSEQ_DUPLICATE:
5936
5937
5938
5939 break;
5940 case L2CAP_TXSEQ_INVALID_IGNORE:
5941 break;
5942 case L2CAP_TXSEQ_INVALID:
5943 default:
5944 l2cap_send_disconn_req(chan, ECONNRESET);
5945 break;
5946 }
5947 break;
5948 case L2CAP_EV_RECV_RR:
5949 l2cap_pass_to_tx(chan, control);
5950 if (control->final) {
5951 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
5952
5953 if (!test_and_clear_bit(CONN_REJ_ACT,
5954 &chan->conn_state)) {
5955 control->final = 0;
5956 l2cap_retransmit_all(chan, control);
5957 }
5958
5959 l2cap_ertm_send(chan);
5960 } else if (control->poll) {
5961 if (test_and_clear_bit(CONN_REMOTE_BUSY,
5962 &chan->conn_state) &&
5963 chan->unacked_frames) {
5964 __set_retrans_timer(chan);
5965 }
5966
5967 set_bit(CONN_SEND_FBIT, &chan->conn_state);
5968 l2cap_send_srej_tail(chan);
5969 } else {
5970 if (test_and_clear_bit(CONN_REMOTE_BUSY,
5971 &chan->conn_state) &&
5972 chan->unacked_frames)
5973 __set_retrans_timer(chan);
5974
5975 l2cap_send_ack(chan);
5976 }
5977 break;
5978 case L2CAP_EV_RECV_RNR:
5979 set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
5980 l2cap_pass_to_tx(chan, control);
5981 if (control->poll) {
5982 l2cap_send_srej_tail(chan);
5983 } else {
5984 struct l2cap_ctrl rr_control;
5985 memset(&rr_control, 0, sizeof(rr_control));
5986 rr_control.sframe = 1;
5987 rr_control.super = L2CAP_SUPER_RR;
5988 rr_control.reqseq = chan->buffer_seq;
5989 l2cap_send_sframe(chan, &rr_control);
5990 }
5991
5992 break;
5993 case L2CAP_EV_RECV_REJ:
5994 l2cap_handle_rej(chan, control);
5995 break;
5996 case L2CAP_EV_RECV_SREJ:
5997 l2cap_handle_srej(chan, control);
5998 break;
5999 }
6000
6001 if (skb && !skb_in_use) {
6002 BT_DBG("Freeing %p", skb);
6003 kfree_skb(skb);
6004 }
6005
6006 return err;
6007}
6008
6009static int l2cap_finish_move(struct l2cap_chan *chan)
6010{
6011 BT_DBG("chan %p", chan);
6012
6013 chan->rx_state = L2CAP_RX_STATE_RECV;
6014
6015 if (chan->hs_hcon)
6016 chan->conn->mtu = chan->hs_hcon->hdev->block_mtu;
6017 else
6018 chan->conn->mtu = chan->conn->hcon->hdev->acl_mtu;
6019
6020 return l2cap_resegment(chan);
6021}
6022
6023static int l2cap_rx_state_wait_p(struct l2cap_chan *chan,
6024 struct l2cap_ctrl *control,
6025 struct sk_buff *skb, u8 event)
6026{
6027 int err;
6028
6029 BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
6030 event);
6031
6032 if (!control->poll)
6033 return -EPROTO;
6034
6035 l2cap_process_reqseq(chan, control->reqseq);
6036
6037 if (!skb_queue_empty(&chan->tx_q))
6038 chan->tx_send_head = skb_peek(&chan->tx_q);
6039 else
6040 chan->tx_send_head = NULL;
6041
6042
6043
6044
6045 chan->next_tx_seq = control->reqseq;
6046 chan->unacked_frames = 0;
6047
6048 err = l2cap_finish_move(chan);
6049 if (err)
6050 return err;
6051
6052 set_bit(CONN_SEND_FBIT, &chan->conn_state);
6053 l2cap_send_i_or_rr_or_rnr(chan);
6054
6055 if (event == L2CAP_EV_RECV_IFRAME)
6056 return -EPROTO;
6057
6058 return l2cap_rx_state_recv(chan, control, NULL, event);
6059}
6060
6061static int l2cap_rx_state_wait_f(struct l2cap_chan *chan,
6062 struct l2cap_ctrl *control,
6063 struct sk_buff *skb, u8 event)
6064{
6065 int err;
6066
6067 if (!control->final)
6068 return -EPROTO;
6069
6070 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6071
6072 chan->rx_state = L2CAP_RX_STATE_RECV;
6073 l2cap_process_reqseq(chan, control->reqseq);
6074
6075 if (!skb_queue_empty(&chan->tx_q))
6076 chan->tx_send_head = skb_peek(&chan->tx_q);
6077 else
6078 chan->tx_send_head = NULL;
6079
6080
6081
6082
6083 chan->next_tx_seq = control->reqseq;
6084 chan->unacked_frames = 0;
6085
6086 if (chan->hs_hcon)
6087 chan->conn->mtu = chan->hs_hcon->hdev->block_mtu;
6088 else
6089 chan->conn->mtu = chan->conn->hcon->hdev->acl_mtu;
6090
6091 err = l2cap_resegment(chan);
6092
6093 if (!err)
6094 err = l2cap_rx_state_recv(chan, control, skb, event);
6095
6096 return err;
6097}
6098
6099static bool __valid_reqseq(struct l2cap_chan *chan, u16 reqseq)
6100{
6101
6102 u16 unacked;
6103
6104 unacked = __seq_offset(chan, chan->next_tx_seq, chan->expected_ack_seq);
6105 return __seq_offset(chan, chan->next_tx_seq, reqseq) <= unacked;
6106}
6107
6108static int l2cap_rx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
6109 struct sk_buff *skb, u8 event)
6110{
6111 int err = 0;
6112
6113 BT_DBG("chan %p, control %p, skb %p, event %d, state %d", chan,
6114 control, skb, event, chan->rx_state);
6115
6116 if (__valid_reqseq(chan, control->reqseq)) {
6117 switch (chan->rx_state) {
6118 case L2CAP_RX_STATE_RECV:
6119 err = l2cap_rx_state_recv(chan, control, skb, event);
6120 break;
6121 case L2CAP_RX_STATE_SREJ_SENT:
6122 err = l2cap_rx_state_srej_sent(chan, control, skb,
6123 event);
6124 break;
6125 case L2CAP_RX_STATE_WAIT_P:
6126 err = l2cap_rx_state_wait_p(chan, control, skb, event);
6127 break;
6128 case L2CAP_RX_STATE_WAIT_F:
6129 err = l2cap_rx_state_wait_f(chan, control, skb, event);
6130 break;
6131 default:
6132
6133 break;
6134 }
6135 } else {
6136 BT_DBG("Invalid reqseq %d (next_tx_seq %d, expected_ack_seq %d",
6137 control->reqseq, chan->next_tx_seq,
6138 chan->expected_ack_seq);
6139 l2cap_send_disconn_req(chan, ECONNRESET);
6140 }
6141
6142 return err;
6143}
6144
6145static int l2cap_stream_rx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
6146 struct sk_buff *skb)
6147{
6148 int err = 0;
6149
6150 BT_DBG("chan %p, control %p, skb %p, state %d", chan, control, skb,
6151 chan->rx_state);
6152
6153 if (l2cap_classify_txseq(chan, control->txseq) ==
6154 L2CAP_TXSEQ_EXPECTED) {
6155 l2cap_pass_to_tx(chan, control);
6156
6157 BT_DBG("buffer_seq %d->%d", chan->buffer_seq,
6158 __next_seq(chan, chan->buffer_seq));
6159
6160 chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
6161
6162 l2cap_reassemble_sdu(chan, skb, control);
6163 } else {
6164 if (chan->sdu) {
6165 kfree_skb(chan->sdu);
6166 chan->sdu = NULL;
6167 }
6168 chan->sdu_last_frag = NULL;
6169 chan->sdu_len = 0;
6170
6171 if (skb) {
6172 BT_DBG("Freeing %p", skb);
6173 kfree_skb(skb);
6174 }
6175 }
6176
6177 chan->last_acked_seq = control->txseq;
6178 chan->expected_tx_seq = __next_seq(chan, control->txseq);
6179
6180 return err;
6181}
6182
6183static int l2cap_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb)
6184{
6185 struct l2cap_ctrl *control = &bt_cb(skb)->control;
6186 u16 len;
6187 u8 event;
6188
6189 __unpack_control(chan, skb);
6190
6191 len = skb->len;
6192
6193
6194
6195
6196
6197
6198 if (l2cap_check_fcs(chan, skb))
6199 goto drop;
6200
6201 if (!control->sframe && control->sar == L2CAP_SAR_START)
6202 len -= L2CAP_SDULEN_SIZE;
6203
6204 if (chan->fcs == L2CAP_FCS_CRC16)
6205 len -= L2CAP_FCS_SIZE;
6206
6207 if (len > chan->mps) {
6208 l2cap_send_disconn_req(chan, ECONNRESET);
6209 goto drop;
6210 }
6211
6212 if (!control->sframe) {
6213 int err;
6214
6215 BT_DBG("iframe sar %d, reqseq %d, final %d, txseq %d",
6216 control->sar, control->reqseq, control->final,
6217 control->txseq);
6218
6219
6220
6221
6222 if (control->final && chan->tx_state != L2CAP_TX_STATE_WAIT_F)
6223 goto drop;
6224
6225 if (chan->mode != L2CAP_MODE_STREAMING) {
6226 event = L2CAP_EV_RECV_IFRAME;
6227 err = l2cap_rx(chan, control, skb, event);
6228 } else {
6229 err = l2cap_stream_rx(chan, control, skb);
6230 }
6231
6232 if (err)
6233 l2cap_send_disconn_req(chan, ECONNRESET);
6234 } else {
6235 const u8 rx_func_to_event[4] = {
6236 L2CAP_EV_RECV_RR, L2CAP_EV_RECV_REJ,
6237 L2CAP_EV_RECV_RNR, L2CAP_EV_RECV_SREJ
6238 };
6239
6240
6241 if (chan->mode == L2CAP_MODE_STREAMING)
6242 goto drop;
6243
6244 BT_DBG("sframe reqseq %d, final %d, poll %d, super %d",
6245 control->reqseq, control->final, control->poll,
6246 control->super);
6247
6248 if (len != 0) {
6249 BT_ERR("Trailing bytes: %d in sframe", len);
6250 l2cap_send_disconn_req(chan, ECONNRESET);
6251 goto drop;
6252 }
6253
6254
6255 if (control->final && (control->poll ||
6256 chan->tx_state != L2CAP_TX_STATE_WAIT_F))
6257 goto drop;
6258
6259 event = rx_func_to_event[control->super];
6260 if (l2cap_rx(chan, control, skb, event))
6261 l2cap_send_disconn_req(chan, ECONNRESET);
6262 }
6263
6264 return 0;
6265
6266drop:
6267 kfree_skb(skb);
6268 return 0;
6269}
6270
6271static void l2cap_data_channel(struct l2cap_conn *conn, u16 cid,
6272 struct sk_buff *skb)
6273{
6274 struct l2cap_chan *chan;
6275
6276 chan = l2cap_get_chan_by_scid(conn, cid);
6277 if (!chan) {
6278 if (cid == L2CAP_CID_A2MP) {
6279 chan = a2mp_channel_create(conn, skb);
6280 if (!chan) {
6281 kfree_skb(skb);
6282 return;
6283 }
6284
6285 l2cap_chan_lock(chan);
6286 } else {
6287 BT_DBG("unknown cid 0x%4.4x", cid);
6288
6289 kfree_skb(skb);
6290 return;
6291 }
6292 }
6293
6294 BT_DBG("chan %p, len %d", chan, skb->len);
6295
6296 if (chan->state != BT_CONNECTED)
6297 goto drop;
6298
6299 switch (chan->mode) {
6300 case L2CAP_MODE_BASIC:
6301
6302
6303
6304
6305
6306 if (chan->imtu < skb->len)
6307 goto drop;
6308
6309 if (!chan->ops->recv(chan, skb))
6310 goto done;
6311 break;
6312
6313 case L2CAP_MODE_ERTM:
6314 case L2CAP_MODE_STREAMING:
6315 l2cap_data_rcv(chan, skb);
6316 goto done;
6317
6318 default:
6319 BT_DBG("chan %p: bad mode 0x%2.2x", chan, chan->mode);
6320 break;
6321 }
6322
6323drop:
6324 kfree_skb(skb);
6325
6326done:
6327 l2cap_chan_unlock(chan);
6328}
6329
6330static void l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm,
6331 struct sk_buff *skb)
6332{
6333 struct l2cap_chan *chan;
6334
6335 chan = l2cap_global_chan_by_psm(0, psm, conn->src, conn->dst);
6336 if (!chan)
6337 goto drop;
6338
6339 BT_DBG("chan %p, len %d", chan, skb->len);
6340
6341 if (chan->state != BT_BOUND && chan->state != BT_CONNECTED)
6342 goto drop;
6343
6344 if (chan->imtu < skb->len)
6345 goto drop;
6346
6347 if (!chan->ops->recv(chan, skb))
6348 return;
6349
6350drop:
6351 kfree_skb(skb);
6352}
6353
6354static void l2cap_att_channel(struct l2cap_conn *conn,
6355 struct sk_buff *skb)
6356{
6357 struct l2cap_chan *chan;
6358
6359 chan = l2cap_global_chan_by_scid(0, L2CAP_CID_LE_DATA,
6360 conn->src, conn->dst);
6361 if (!chan)
6362 goto drop;
6363
6364 BT_DBG("chan %p, len %d", chan, skb->len);
6365
6366 if (chan->state != BT_BOUND && chan->state != BT_CONNECTED)
6367 goto drop;
6368
6369 if (chan->imtu < skb->len)
6370 goto drop;
6371
6372 if (!chan->ops->recv(chan, skb))
6373 return;
6374
6375drop:
6376 kfree_skb(skb);
6377}
6378
6379static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
6380{
6381 struct l2cap_hdr *lh = (void *) skb->data;
6382 u16 cid, len;
6383 __le16 psm;
6384
6385 skb_pull(skb, L2CAP_HDR_SIZE);
6386 cid = __le16_to_cpu(lh->cid);
6387 len = __le16_to_cpu(lh->len);
6388
6389 if (len != skb->len) {
6390 kfree_skb(skb);
6391 return;
6392 }
6393
6394 BT_DBG("len %d, cid 0x%4.4x", len, cid);
6395
6396 switch (cid) {
6397 case L2CAP_CID_LE_SIGNALING:
6398 case L2CAP_CID_SIGNALING:
6399 l2cap_sig_channel(conn, skb);
6400 break;
6401
6402 case L2CAP_CID_CONN_LESS:
6403 psm = get_unaligned((__le16 *) skb->data);
6404 skb_pull(skb, L2CAP_PSMLEN_SIZE);
6405 l2cap_conless_channel(conn, psm, skb);
6406 break;
6407
6408 case L2CAP_CID_LE_DATA:
6409 l2cap_att_channel(conn, skb);
6410 break;
6411
6412 case L2CAP_CID_SMP:
6413 if (smp_sig_channel(conn, skb))
6414 l2cap_conn_del(conn->hcon, EACCES);
6415 break;
6416
6417 default:
6418 l2cap_data_channel(conn, cid, skb);
6419 break;
6420 }
6421}
6422
6423
6424
6425int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr)
6426{
6427 int exact = 0, lm1 = 0, lm2 = 0;
6428 struct l2cap_chan *c;
6429
6430 BT_DBG("hdev %s, bdaddr %pMR", hdev->name, bdaddr);
6431
6432
6433 read_lock(&chan_list_lock);
6434 list_for_each_entry(c, &chan_list, global_l) {
6435 struct sock *sk = c->sk;
6436
6437 if (c->state != BT_LISTEN)
6438 continue;
6439
6440 if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
6441 lm1 |= HCI_LM_ACCEPT;
6442 if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
6443 lm1 |= HCI_LM_MASTER;
6444 exact++;
6445 } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) {
6446 lm2 |= HCI_LM_ACCEPT;
6447 if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
6448 lm2 |= HCI_LM_MASTER;
6449 }
6450 }
6451 read_unlock(&chan_list_lock);
6452
6453 return exact ? lm1 : lm2;
6454}
6455
6456void l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
6457{
6458 struct l2cap_conn *conn;
6459
6460 BT_DBG("hcon %p bdaddr %pMR status %d", hcon, &hcon->dst, status);
6461
6462 if (!status) {
6463 conn = l2cap_conn_add(hcon);
6464 if (conn)
6465 l2cap_conn_ready(conn);
6466 } else {
6467 l2cap_conn_del(hcon, bt_to_errno(status));
6468 }
6469}
6470
6471int l2cap_disconn_ind(struct hci_conn *hcon)
6472{
6473 struct l2cap_conn *conn = hcon->l2cap_data;
6474
6475 BT_DBG("hcon %p", hcon);
6476
6477 if (!conn)
6478 return HCI_ERROR_REMOTE_USER_TERM;
6479 return conn->disc_reason;
6480}
6481
6482void l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
6483{
6484 BT_DBG("hcon %p reason %d", hcon, reason);
6485
6486 l2cap_conn_del(hcon, bt_to_errno(reason));
6487}
6488
6489static inline void l2cap_check_encryption(struct l2cap_chan *chan, u8 encrypt)
6490{
6491 if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED)
6492 return;
6493
6494 if (encrypt == 0x00) {
6495 if (chan->sec_level == BT_SECURITY_MEDIUM) {
6496 __set_chan_timer(chan, L2CAP_ENC_TIMEOUT);
6497 } else if (chan->sec_level == BT_SECURITY_HIGH)
6498 l2cap_chan_close(chan, ECONNREFUSED);
6499 } else {
6500 if (chan->sec_level == BT_SECURITY_MEDIUM)
6501 __clear_chan_timer(chan);
6502 }
6503}
6504
6505int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
6506{
6507 struct l2cap_conn *conn = hcon->l2cap_data;
6508 struct l2cap_chan *chan;
6509
6510 if (!conn)
6511 return 0;
6512
6513 BT_DBG("conn %p status 0x%2.2x encrypt %u", conn, status, encrypt);
6514
6515 if (hcon->type == LE_LINK) {
6516 if (!status && encrypt)
6517 smp_distribute_keys(conn, 0);
6518 cancel_delayed_work(&conn->security_timer);
6519 }
6520
6521 mutex_lock(&conn->chan_lock);
6522
6523 list_for_each_entry(chan, &conn->chan_l, list) {
6524 l2cap_chan_lock(chan);
6525
6526 BT_DBG("chan %p scid 0x%4.4x state %s", chan, chan->scid,
6527 state_to_string(chan->state));
6528
6529 if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP) {
6530 l2cap_chan_unlock(chan);
6531 continue;
6532 }
6533
6534 if (chan->scid == L2CAP_CID_LE_DATA) {
6535 if (!status && encrypt) {
6536 chan->sec_level = hcon->sec_level;
6537 l2cap_chan_ready(chan);
6538 }
6539
6540 l2cap_chan_unlock(chan);
6541 continue;
6542 }
6543
6544 if (!__l2cap_no_conn_pending(chan)) {
6545 l2cap_chan_unlock(chan);
6546 continue;
6547 }
6548
6549 if (!status && (chan->state == BT_CONNECTED ||
6550 chan->state == BT_CONFIG)) {
6551 struct sock *sk = chan->sk;
6552
6553 clear_bit(BT_SK_SUSPEND, &bt_sk(sk)->flags);
6554 sk->sk_state_change(sk);
6555
6556 l2cap_check_encryption(chan, encrypt);
6557 l2cap_chan_unlock(chan);
6558 continue;
6559 }
6560
6561 if (chan->state == BT_CONNECT) {
6562 if (!status) {
6563 l2cap_start_connection(chan);
6564 } else {
6565 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
6566 }
6567 } else if (chan->state == BT_CONNECT2) {
6568 struct sock *sk = chan->sk;
6569 struct l2cap_conn_rsp rsp;
6570 __u16 res, stat;
6571
6572 lock_sock(sk);
6573
6574 if (!status) {
6575 if (test_bit(BT_SK_DEFER_SETUP,
6576 &bt_sk(sk)->flags)) {
6577 res = L2CAP_CR_PEND;
6578 stat = L2CAP_CS_AUTHOR_PEND;
6579 chan->ops->defer(chan);
6580 } else {
6581 __l2cap_state_change(chan, BT_CONFIG);
6582 res = L2CAP_CR_SUCCESS;
6583 stat = L2CAP_CS_NO_INFO;
6584 }
6585 } else {
6586 __l2cap_state_change(chan, BT_DISCONN);
6587 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
6588 res = L2CAP_CR_SEC_BLOCK;
6589 stat = L2CAP_CS_NO_INFO;
6590 }
6591
6592 release_sock(sk);
6593
6594 rsp.scid = cpu_to_le16(chan->dcid);
6595 rsp.dcid = cpu_to_le16(chan->scid);
6596 rsp.result = cpu_to_le16(res);
6597 rsp.status = cpu_to_le16(stat);
6598 l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
6599 sizeof(rsp), &rsp);
6600
6601 if (!test_bit(CONF_REQ_SENT, &chan->conf_state) &&
6602 res == L2CAP_CR_SUCCESS) {
6603 char buf[128];
6604 set_bit(CONF_REQ_SENT, &chan->conf_state);
6605 l2cap_send_cmd(conn, l2cap_get_ident(conn),
6606 L2CAP_CONF_REQ,
6607 l2cap_build_conf_req(chan, buf),
6608 buf);
6609 chan->num_conf_req++;
6610 }
6611 }
6612
6613 l2cap_chan_unlock(chan);
6614 }
6615
6616 mutex_unlock(&conn->chan_lock);
6617
6618 return 0;
6619}
6620
6621int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
6622{
6623 struct l2cap_conn *conn = hcon->l2cap_data;
6624 struct l2cap_hdr *hdr;
6625 int len;
6626
6627
6628 if (!conn && hcon->hdev->dev_type != HCI_BREDR)
6629 goto drop;
6630
6631 if (!conn)
6632 conn = l2cap_conn_add(hcon);
6633
6634 if (!conn)
6635 goto drop;
6636
6637 BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
6638
6639 switch (flags) {
6640 case ACL_START:
6641 case ACL_START_NO_FLUSH:
6642 case ACL_COMPLETE:
6643 if (conn->rx_len) {
6644 BT_ERR("Unexpected start frame (len %d)", skb->len);
6645 kfree_skb(conn->rx_skb);
6646 conn->rx_skb = NULL;
6647 conn->rx_len = 0;
6648 l2cap_conn_unreliable(conn, ECOMM);
6649 }
6650
6651
6652 if (skb->len < L2CAP_HDR_SIZE) {
6653 BT_ERR("Frame is too short (len %d)", skb->len);
6654 l2cap_conn_unreliable(conn, ECOMM);
6655 goto drop;
6656 }
6657
6658 hdr = (struct l2cap_hdr *) skb->data;
6659 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
6660
6661 if (len == skb->len) {
6662
6663 l2cap_recv_frame(conn, skb);
6664 return 0;
6665 }
6666
6667 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
6668
6669 if (skb->len > len) {
6670 BT_ERR("Frame is too long (len %d, expected len %d)",
6671 skb->len, len);
6672 l2cap_conn_unreliable(conn, ECOMM);
6673 goto drop;
6674 }
6675
6676
6677 conn->rx_skb = bt_skb_alloc(len, GFP_KERNEL);
6678 if (!conn->rx_skb)
6679 goto drop;
6680
6681 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
6682 skb->len);
6683 conn->rx_len = len - skb->len;
6684 break;
6685
6686 case ACL_CONT:
6687 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
6688
6689 if (!conn->rx_len) {
6690 BT_ERR("Unexpected continuation frame (len %d)", skb->len);
6691 l2cap_conn_unreliable(conn, ECOMM);
6692 goto drop;
6693 }
6694
6695 if (skb->len > conn->rx_len) {
6696 BT_ERR("Fragment is too long (len %d, expected %d)",
6697 skb->len, conn->rx_len);
6698 kfree_skb(conn->rx_skb);
6699 conn->rx_skb = NULL;
6700 conn->rx_len = 0;
6701 l2cap_conn_unreliable(conn, ECOMM);
6702 goto drop;
6703 }
6704
6705 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
6706 skb->len);
6707 conn->rx_len -= skb->len;
6708
6709 if (!conn->rx_len) {
6710
6711 l2cap_recv_frame(conn, conn->rx_skb);
6712 conn->rx_skb = NULL;
6713 }
6714 break;
6715 }
6716
6717drop:
6718 kfree_skb(skb);
6719 return 0;
6720}
6721
6722static int l2cap_debugfs_show(struct seq_file *f, void *p)
6723{
6724 struct l2cap_chan *c;
6725
6726 read_lock(&chan_list_lock);
6727
6728 list_for_each_entry(c, &chan_list, global_l) {
6729 struct sock *sk = c->sk;
6730
6731 seq_printf(f, "%pMR %pMR %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n",
6732 &bt_sk(sk)->src, &bt_sk(sk)->dst,
6733 c->state, __le16_to_cpu(c->psm),
6734 c->scid, c->dcid, c->imtu, c->omtu,
6735 c->sec_level, c->mode);
6736 }
6737
6738 read_unlock(&chan_list_lock);
6739
6740 return 0;
6741}
6742
6743static int l2cap_debugfs_open(struct inode *inode, struct file *file)
6744{
6745 return single_open(file, l2cap_debugfs_show, inode->i_private);
6746}
6747
6748static const struct file_operations l2cap_debugfs_fops = {
6749 .open = l2cap_debugfs_open,
6750 .read = seq_read,
6751 .llseek = seq_lseek,
6752 .release = single_release,
6753};
6754
6755static struct dentry *l2cap_debugfs;
6756
6757int __init l2cap_init(void)
6758{
6759 int err;
6760
6761 err = l2cap_init_sockets();
6762 if (err < 0)
6763 return err;
6764
6765 if (bt_debugfs) {
6766 l2cap_debugfs = debugfs_create_file("l2cap", 0444, bt_debugfs,
6767 NULL, &l2cap_debugfs_fops);
6768 if (!l2cap_debugfs)
6769 BT_ERR("Failed to create L2CAP debug file");
6770 }
6771
6772 return 0;
6773}
6774
6775void l2cap_exit(void)
6776{
6777 debugfs_remove(l2cap_debugfs);
6778 l2cap_cleanup_sockets();
6779}
6780
6781module_param(disable_ertm, bool, 0644);
6782MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode");
6783