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