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