?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
staging: ft1000: Removed unused variable InterruptsEnabled.
[linux-2.6.git] / net / bluetooth / l2cap_core.c
blobca27f3a41536f35d4d608f9f0929b50105658f4d
1 /*
2 BlueZ - Bluetooth protocol stack for Linux
3 Copyright (C) 2000-2001 Qualcomm Incorporated
4 Copyright (C) 2009-2010 Gustavo F. Padovan <gustavo@padovan.org>
5 Copyright (C) 2010 Google Inc.
6
7 Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.com>
8
9 This program is free software; you can redistribute it and/or modify
10 it under the terms of the GNU General Public License version 2 as
11 published by the Free Software Foundation;
12
13 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
14 OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS.
16 IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY
17 CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES
18 WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
19 ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
20 OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
21
22 ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS,
23 COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS
24 SOFTWARE IS DISCLAIMED.
25 */
26
27 /* Bluetooth L2CAP core. */
28
29 #include <linux/module.h>
30
31 #include <linux/types.h>
32 #include <linux/capability.h>
33 #include <linux/errno.h>
34 #include <linux/kernel.h>
35 #include <linux/sched.h>
36 #include <linux/slab.h>
37 #include <linux/poll.h>
38 #include <linux/fcntl.h>
39 #include <linux/init.h>
40 #include <linux/interrupt.h>
41 #include <linux/socket.h>
42 #include <linux/skbuff.h>
43 #include <linux/list.h>
44 #include <linux/device.h>
45 #include <linux/debugfs.h>
46 #include <linux/seq_file.h>
47 #include <linux/uaccess.h>
48 #include <linux/crc16.h>
49 #include <net/sock.h>
50
51 #include <asm/system.h>
52 #include <asm/unaligned.h>
53
54 #include <net/bluetooth/bluetooth.h>
55 #include <net/bluetooth/hci_core.h>
56 #include <net/bluetooth/l2cap.h>
57
58 int disable_ertm;
59
60 static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN;
61 static u8 l2cap_fixed_chan[8] = { 0x02, };
62
63 static struct workqueue_struct *_busy_wq;
64
65 struct bt_sock_list l2cap_sk_list = {
66 .lock = __RW_LOCK_UNLOCKED(l2cap_sk_list.lock)
67 };
68
69 static void l2cap_busy_work(struct work_struct *work);
70
71 static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
72 u8 code, u8 ident, u16 dlen, void *data);
73
74 static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb);
75
76 /* ---- L2CAP channels ---- */
77 static struct sock *__l2cap_get_chan_by_dcid(struct l2cap_chan_list *l, u16 cid)
78 {
79 struct sock *s;
80 for (s = l->head; s; s = l2cap_pi(s)->next_c) {
81 if (l2cap_pi(s)->dcid == cid)
82 break;
83 }
84 return s;
85 }
86
87 static struct sock *__l2cap_get_chan_by_scid(struct l2cap_chan_list *l, u16 cid)
88 {
89 struct sock *s;
90 for (s = l->head; s; s = l2cap_pi(s)->next_c) {
91 if (l2cap_pi(s)->scid == cid)
92 break;
93 }
94 return s;
95 }
96
97 /* Find channel with given SCID.
98 * Returns locked socket */
99 static inline struct sock *l2cap_get_chan_by_scid(struct l2cap_chan_list *l, u16 cid)
100 {
101 struct sock *s;
102 read_lock(&l->lock);
103 s = __l2cap_get_chan_by_scid(l, cid);
104 if (s)
105 bh_lock_sock(s);
106 read_unlock(&l->lock);
107 return s;
108 }
109
110 static struct sock *__l2cap_get_chan_by_ident(struct l2cap_chan_list *l, u8 ident)
111 {
112 struct sock *s;
113 for (s = l->head; s; s = l2cap_pi(s)->next_c) {
114 if (l2cap_pi(s)->ident == ident)
115 break;
116 }
117 return s;
118 }
119
120 static inline struct sock *l2cap_get_chan_by_ident(struct l2cap_chan_list *l, u8 ident)
121 {
122 struct sock *s;
123 read_lock(&l->lock);
124 s = __l2cap_get_chan_by_ident(l, ident);
125 if (s)
126 bh_lock_sock(s);
127 read_unlock(&l->lock);
128 return s;
129 }
130
131 static u16 l2cap_alloc_cid(struct l2cap_chan_list *l)
132 {
133 u16 cid = L2CAP_CID_DYN_START;
134
135 for (; cid < L2CAP_CID_DYN_END; cid++) {
136 if (!__l2cap_get_chan_by_scid(l, cid))
137 return cid;
138 }
139
140 return 0;
141 }
142
143 static inline void __l2cap_chan_link(struct l2cap_chan_list *l, struct sock *sk)
144 {
145 sock_hold(sk);
146
147 if (l->head)
148 l2cap_pi(l->head)->prev_c = sk;
149
150 l2cap_pi(sk)->next_c = l->head;
151 l2cap_pi(sk)->prev_c = NULL;
152 l->head = sk;
153 }
154
155 static inline void l2cap_chan_unlink(struct l2cap_chan_list *l, struct sock *sk)
156 {
157 struct sock *next = l2cap_pi(sk)->next_c, *prev = l2cap_pi(sk)->prev_c;
158
159 write_lock_bh(&l->lock);
160 if (sk == l->head)
161 l->head = next;
162
163 if (next)
164 l2cap_pi(next)->prev_c = prev;
165 if (prev)
166 l2cap_pi(prev)->next_c = next;
167 write_unlock_bh(&l->lock);
168
169 __sock_put(sk);
170 }
171
172 static void __l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct sock *parent)
173 {
174 struct l2cap_chan_list *l = &conn->chan_list;
175
176 BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn,
177 l2cap_pi(sk)->psm, l2cap_pi(sk)->dcid);
178
179 conn->disc_reason = 0x13;
180
181 l2cap_pi(sk)->conn = conn;
182
183 if (sk->sk_type == SOCK_SEQPACKET || sk->sk_type == SOCK_STREAM) {
184 if (conn->hcon->type == LE_LINK) {
185 /* LE connection */
186 l2cap_pi(sk)->omtu = L2CAP_LE_DEFAULT_MTU;
187 l2cap_pi(sk)->scid = L2CAP_CID_LE_DATA;
188 l2cap_pi(sk)->dcid = L2CAP_CID_LE_DATA;
189 } else {
190 /* Alloc CID for connection-oriented socket */
191 l2cap_pi(sk)->scid = l2cap_alloc_cid(l);
192 l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU;
193 }
194 } else if (sk->sk_type == SOCK_DGRAM) {
195 /* Connectionless socket */
196 l2cap_pi(sk)->scid = L2CAP_CID_CONN_LESS;
197 l2cap_pi(sk)->dcid = L2CAP_CID_CONN_LESS;
198 l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU;
199 } else {
200 /* Raw socket can send/recv signalling messages only */
201 l2cap_pi(sk)->scid = L2CAP_CID_SIGNALING;
202 l2cap_pi(sk)->dcid = L2CAP_CID_SIGNALING;
203 l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU;
204 }
205
206 __l2cap_chan_link(l, sk);
207
208 if (parent)
209 bt_accept_enqueue(parent, sk);
210 }
211
212 /* Delete channel.
213 * Must be called on the locked socket. */
214 void l2cap_chan_del(struct sock *sk, int err)
215 {
216 struct l2cap_conn *conn = l2cap_pi(sk)->conn;
217 struct sock *parent = bt_sk(sk)->parent;
218
219 l2cap_sock_clear_timer(sk);
220
221 BT_DBG("sk %p, conn %p, err %d", sk, conn, err);
222
223 if (conn) {
224 /* Unlink from channel list */
225 l2cap_chan_unlink(&conn->chan_list, sk);
226 l2cap_pi(sk)->conn = NULL;
227 hci_conn_put(conn->hcon);
228 }
229
230 sk->sk_state = BT_CLOSED;
231 sock_set_flag(sk, SOCK_ZAPPED);
232
233 if (err)
234 sk->sk_err = err;
235
236 if (parent) {
237 bt_accept_unlink(sk);
238 parent->sk_data_ready(parent, 0);
239 } else
240 sk->sk_state_change(sk);
241
242 skb_queue_purge(TX_QUEUE(sk));
243
244 if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM) {
245 struct srej_list *l, *tmp;
246
247 del_timer(&l2cap_pi(sk)->retrans_timer);
248 del_timer(&l2cap_pi(sk)->monitor_timer);
249 del_timer(&l2cap_pi(sk)->ack_timer);
250
251 skb_queue_purge(SREJ_QUEUE(sk));
252 skb_queue_purge(BUSY_QUEUE(sk));
253
254 list_for_each_entry_safe(l, tmp, SREJ_LIST(sk), list) {
255 list_del(&l->list);
256 kfree(l);
257 }
258 }
259 }
260
261 static inline u8 l2cap_get_auth_type(struct sock *sk)
262 {
263 if (sk->sk_type == SOCK_RAW) {
264 switch (l2cap_pi(sk)->sec_level) {
265 case BT_SECURITY_HIGH:
266 return HCI_AT_DEDICATED_BONDING_MITM;
267 case BT_SECURITY_MEDIUM:
268 return HCI_AT_DEDICATED_BONDING;
269 default:
270 return HCI_AT_NO_BONDING;
271 }
272 } else if (l2cap_pi(sk)->psm == cpu_to_le16(0x0001)) {
273 if (l2cap_pi(sk)->sec_level == BT_SECURITY_LOW)
274 l2cap_pi(sk)->sec_level = BT_SECURITY_SDP;
275
276 if (l2cap_pi(sk)->sec_level == BT_SECURITY_HIGH)
277 return HCI_AT_NO_BONDING_MITM;
278 else
279 return HCI_AT_NO_BONDING;
280 } else {
281 switch (l2cap_pi(sk)->sec_level) {
282 case BT_SECURITY_HIGH:
283 return HCI_AT_GENERAL_BONDING_MITM;
284 case BT_SECURITY_MEDIUM:
285 return HCI_AT_GENERAL_BONDING;
286 default:
287 return HCI_AT_NO_BONDING;
288 }
289 }
290 }
291
292 /* Service level security */
293 static inline int l2cap_check_security(struct sock *sk)
294 {
295 struct l2cap_conn *conn = l2cap_pi(sk)->conn;
296 __u8 auth_type;
297
298 auth_type = l2cap_get_auth_type(sk);
299
300 return hci_conn_security(conn->hcon, l2cap_pi(sk)->sec_level,
301 auth_type);
302 }
303
304 u8 l2cap_get_ident(struct l2cap_conn *conn)
305 {
306 u8 id;
307
308 /* Get next available identificator.
309 * 1 - 128 are used by kernel.
310 * 129 - 199 are reserved.
311 * 200 - 254 are used by utilities like l2ping, etc.
312 */
313
314 spin_lock_bh(&conn->lock);
315
316 if (++conn->tx_ident > 128)
317 conn->tx_ident = 1;
318
319 id = conn->tx_ident;
320
321 spin_unlock_bh(&conn->lock);
322
323 return id;
324 }
325
326 void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, void *data)
327 {
328 struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data);
329 u8 flags;
330
331 BT_DBG("code 0x%2.2x", code);
332
333 if (!skb)
334 return;
335
336 if (lmp_no_flush_capable(conn->hcon->hdev))
337 flags = ACL_START_NO_FLUSH;
338 else
339 flags = ACL_START;
340
341 hci_send_acl(conn->hcon, skb, flags);
342 }
343
344 static inline void l2cap_send_sframe(struct l2cap_pinfo *pi, u16 control)
345 {
346 struct sk_buff *skb;
347 struct l2cap_hdr *lh;
348 struct l2cap_conn *conn = pi->conn;
349 struct sock *sk = (struct sock *)pi;
350 int count, hlen = L2CAP_HDR_SIZE + 2;
351 u8 flags;
352
353 if (sk->sk_state != BT_CONNECTED)
354 return;
355
356 if (pi->fcs == L2CAP_FCS_CRC16)
357 hlen += 2;
358
359 BT_DBG("pi %p, control 0x%2.2x", pi, control);
360
361 count = min_t(unsigned int, conn->mtu, hlen);
362 control |= L2CAP_CTRL_FRAME_TYPE;
363
364 if (pi->conn_state & L2CAP_CONN_SEND_FBIT) {
365 control |= L2CAP_CTRL_FINAL;
366 pi->conn_state &= ~L2CAP_CONN_SEND_FBIT;
367 }
368
369 if (pi->conn_state & L2CAP_CONN_SEND_PBIT) {
370 control |= L2CAP_CTRL_POLL;
371 pi->conn_state &= ~L2CAP_CONN_SEND_PBIT;
372 }
373
374 skb = bt_skb_alloc(count, GFP_ATOMIC);
375 if (!skb)
376 return;
377
378 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
379 lh->len = cpu_to_le16(hlen - L2CAP_HDR_SIZE);
380 lh->cid = cpu_to_le16(pi->dcid);
381 put_unaligned_le16(control, skb_put(skb, 2));
382
383 if (pi->fcs == L2CAP_FCS_CRC16) {
384 u16 fcs = crc16(0, (u8 *)lh, count - 2);
385 put_unaligned_le16(fcs, skb_put(skb, 2));
386 }
387
388 if (lmp_no_flush_capable(conn->hcon->hdev))
389 flags = ACL_START_NO_FLUSH;
390 else
391 flags = ACL_START;
392
393 hci_send_acl(pi->conn->hcon, skb, flags);
394 }
395
396 static inline void l2cap_send_rr_or_rnr(struct l2cap_pinfo *pi, u16 control)
397 {
398 if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
399 control |= L2CAP_SUPER_RCV_NOT_READY;
400 pi->conn_state |= L2CAP_CONN_RNR_SENT;
401 } else
402 control |= L2CAP_SUPER_RCV_READY;
403
404 control |= pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
405
406 l2cap_send_sframe(pi, control);
407 }
408
409 static inline int __l2cap_no_conn_pending(struct sock *sk)
410 {
411 return !(l2cap_pi(sk)->conf_state & L2CAP_CONF_CONNECT_PEND);
412 }
413
414 static void l2cap_do_start(struct sock *sk)
415 {
416 struct l2cap_conn *conn = l2cap_pi(sk)->conn;
417
418 if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) {
419 if (!(conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE))
420 return;
421
422 if (l2cap_check_security(sk) && __l2cap_no_conn_pending(sk)) {
423 struct l2cap_conn_req req;
424 req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
425 req.psm = l2cap_pi(sk)->psm;
426
427 l2cap_pi(sk)->ident = l2cap_get_ident(conn);
428 l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND;
429
430 l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
431 L2CAP_CONN_REQ, sizeof(req), &req);
432 }
433 } else {
434 struct l2cap_info_req req;
435 req.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
436
437 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
438 conn->info_ident = l2cap_get_ident(conn);
439
440 mod_timer(&conn->info_timer, jiffies +
441 msecs_to_jiffies(L2CAP_INFO_TIMEOUT));
442
443 l2cap_send_cmd(conn, conn->info_ident,
444 L2CAP_INFO_REQ, sizeof(req), &req);
445 }
446 }
447
448 static inline int l2cap_mode_supported(__u8 mode, __u32 feat_mask)
449 {
450 u32 local_feat_mask = l2cap_feat_mask;
451 if (!disable_ertm)
452 local_feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING;
453
454 switch (mode) {
455 case L2CAP_MODE_ERTM:
456 return L2CAP_FEAT_ERTM & feat_mask & local_feat_mask;
457 case L2CAP_MODE_STREAMING:
458 return L2CAP_FEAT_STREAMING & feat_mask & local_feat_mask;
459 default:
460 return 0x00;
461 }
462 }
463
464 void l2cap_send_disconn_req(struct l2cap_conn *conn, struct sock *sk, int err)
465 {
466 struct l2cap_disconn_req req;
467
468 if (!conn)
469 return;
470
471 skb_queue_purge(TX_QUEUE(sk));
472
473 if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM) {
474 del_timer(&l2cap_pi(sk)->retrans_timer);
475 del_timer(&l2cap_pi(sk)->monitor_timer);
476 del_timer(&l2cap_pi(sk)->ack_timer);
477 }
478
479 req.dcid = cpu_to_le16(l2cap_pi(sk)->dcid);
480 req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
481 l2cap_send_cmd(conn, l2cap_get_ident(conn),
482 L2CAP_DISCONN_REQ, sizeof(req), &req);
483
484 sk->sk_state = BT_DISCONN;
485 sk->sk_err = err;
486 }
487
488 /* ---- L2CAP connections ---- */
489 static void l2cap_conn_start(struct l2cap_conn *conn)
490 {
491 struct l2cap_chan_list *l = &conn->chan_list;
492 struct sock_del_list del, *tmp1, *tmp2;
493 struct sock *sk;
494
495 BT_DBG("conn %p", conn);
496
497 INIT_LIST_HEAD(&del.list);
498
499 read_lock(&l->lock);
500
501 for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
502 bh_lock_sock(sk);
503
504 if (sk->sk_type != SOCK_SEQPACKET &&
505 sk->sk_type != SOCK_STREAM) {
506 bh_unlock_sock(sk);
507 continue;
508 }
509
510 if (sk->sk_state == BT_CONNECT) {
511 struct l2cap_conn_req req;
512
513 if (!l2cap_check_security(sk) ||
514 !__l2cap_no_conn_pending(sk)) {
515 bh_unlock_sock(sk);
516 continue;
517 }
518
519 if (!l2cap_mode_supported(l2cap_pi(sk)->mode,
520 conn->feat_mask)
521 && l2cap_pi(sk)->conf_state &
522 L2CAP_CONF_STATE2_DEVICE) {
523 tmp1 = kzalloc(sizeof(struct sock_del_list),
524 GFP_ATOMIC);
525 tmp1->sk = sk;
526 list_add_tail(&tmp1->list, &del.list);
527 bh_unlock_sock(sk);
528 continue;
529 }
530
531 req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
532 req.psm = l2cap_pi(sk)->psm;
533
534 l2cap_pi(sk)->ident = l2cap_get_ident(conn);
535 l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND;
536
537 l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
538 L2CAP_CONN_REQ, sizeof(req), &req);
539
540 } else if (sk->sk_state == BT_CONNECT2) {
541 struct l2cap_conn_rsp rsp;
542 char buf[128];
543 rsp.scid = cpu_to_le16(l2cap_pi(sk)->dcid);
544 rsp.dcid = cpu_to_le16(l2cap_pi(sk)->scid);
545
546 if (l2cap_check_security(sk)) {
547 if (bt_sk(sk)->defer_setup) {
548 struct sock *parent = bt_sk(sk)->parent;
549 rsp.result = cpu_to_le16(L2CAP_CR_PEND);
550 rsp.status = cpu_to_le16(L2CAP_CS_AUTHOR_PEND);
551 parent->sk_data_ready(parent, 0);
552
553 } else {
554 sk->sk_state = BT_CONFIG;
555 rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
556 rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
557 }
558 } else {
559 rsp.result = cpu_to_le16(L2CAP_CR_PEND);
560 rsp.status = cpu_to_le16(L2CAP_CS_AUTHEN_PEND);
561 }
562
563 l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
564 L2CAP_CONN_RSP, sizeof(rsp), &rsp);
565
566 if (l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT ||
567 rsp.result != L2CAP_CR_SUCCESS) {
568 bh_unlock_sock(sk);
569 continue;
570 }
571
572 l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
573 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
574 l2cap_build_conf_req(sk, buf), buf);
575 l2cap_pi(sk)->num_conf_req++;
576 }
577
578 bh_unlock_sock(sk);
579 }
580
581 read_unlock(&l->lock);
582
583 list_for_each_entry_safe(tmp1, tmp2, &del.list, list) {
584 bh_lock_sock(tmp1->sk);
585 __l2cap_sock_close(tmp1->sk, ECONNRESET);
586 bh_unlock_sock(tmp1->sk);
587 list_del(&tmp1->list);
588 kfree(tmp1);
589 }
590 }
591
592 /* Find socket with cid and source bdaddr.
593 * Returns closest match, locked.
594 */
595 static struct sock *l2cap_get_sock_by_scid(int state, __le16 cid, bdaddr_t *src)
596 {
597 struct sock *s, *sk = NULL, *sk1 = NULL;
598 struct hlist_node *node;
599
600 read_lock(&l2cap_sk_list.lock);
601
602 sk_for_each(sk, node, &l2cap_sk_list.head) {
603 if (state && sk->sk_state != state)
604 continue;
605
606 if (l2cap_pi(sk)->scid == cid) {
607 /* Exact match. */
608 if (!bacmp(&bt_sk(sk)->src, src))
609 break;
610
611 /* Closest match */
612 if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY))
613 sk1 = sk;
614 }
615 }
616 s = node ? sk : sk1;
617 if (s)
618 bh_lock_sock(s);
619 read_unlock(&l2cap_sk_list.lock);
620
621 return s;
622 }
623
624 static void l2cap_le_conn_ready(struct l2cap_conn *conn)
625 {
626 struct l2cap_chan_list *list = &conn->chan_list;
627 struct sock *parent, *uninitialized_var(sk);
628
629 BT_DBG("");
630
631 /* Check if we have socket listening on cid */
632 parent = l2cap_get_sock_by_scid(BT_LISTEN, L2CAP_CID_LE_DATA,
633 conn->src);
634 if (!parent)
635 return;
636
637 /* Check for backlog size */
638 if (sk_acceptq_is_full(parent)) {
639 BT_DBG("backlog full %d", parent->sk_ack_backlog);
640 goto clean;
641 }
642
643 sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP, GFP_ATOMIC);
644 if (!sk)
645 goto clean;
646
647 write_lock_bh(&list->lock);
648
649 hci_conn_hold(conn->hcon);
650
651 l2cap_sock_init(sk, parent);
652 bacpy(&bt_sk(sk)->src, conn->src);
653 bacpy(&bt_sk(sk)->dst, conn->dst);
654
655 __l2cap_chan_add(conn, sk, parent);
656
657 l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
658
659 sk->sk_state = BT_CONNECTED;
660 parent->sk_data_ready(parent, 0);
661
662 write_unlock_bh(&list->lock);
663
664 clean:
665 bh_unlock_sock(parent);
666 }
667
668 static void l2cap_conn_ready(struct l2cap_conn *conn)
669 {
670 struct l2cap_chan_list *l = &conn->chan_list;
671 struct sock *sk;
672
673 BT_DBG("conn %p", conn);
674
675 if (!conn->hcon->out && conn->hcon->type == LE_LINK)
676 l2cap_le_conn_ready(conn);
677
678 read_lock(&l->lock);
679
680 for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
681 bh_lock_sock(sk);
682
683 if (conn->hcon->type == LE_LINK) {
684 l2cap_sock_clear_timer(sk);
685 sk->sk_state = BT_CONNECTED;
686 sk->sk_state_change(sk);
687 }
688
689 if (sk->sk_type != SOCK_SEQPACKET &&
690 sk->sk_type != SOCK_STREAM) {
691 l2cap_sock_clear_timer(sk);
692 sk->sk_state = BT_CONNECTED;
693 sk->sk_state_change(sk);
694 } else if (sk->sk_state == BT_CONNECT)
695 l2cap_do_start(sk);
696
697 bh_unlock_sock(sk);
698 }
699
700 read_unlock(&l->lock);
701 }
702
703 /* Notify sockets that we cannot guaranty reliability anymore */
704 static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err)
705 {
706 struct l2cap_chan_list *l = &conn->chan_list;
707 struct sock *sk;
708
709 BT_DBG("conn %p", conn);
710
711 read_lock(&l->lock);
712
713 for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
714 if (l2cap_pi(sk)->force_reliable)
715 sk->sk_err = err;
716 }
717
718 read_unlock(&l->lock);
719 }
720
721 static void l2cap_info_timeout(unsigned long arg)
722 {
723 struct l2cap_conn *conn = (void *) arg;
724
725 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
726 conn->info_ident = 0;
727
728 l2cap_conn_start(conn);
729 }
730
731 static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status)
732 {
733 struct l2cap_conn *conn = hcon->l2cap_data;
734
735 if (conn || status)
736 return conn;
737
738 conn = kzalloc(sizeof(struct l2cap_conn), GFP_ATOMIC);
739 if (!conn)
740 return NULL;
741
742 hcon->l2cap_data = conn;
743 conn->hcon = hcon;
744
745 BT_DBG("hcon %p conn %p", hcon, conn);
746
747 if (hcon->hdev->le_mtu && hcon->type == LE_LINK)
748 conn->mtu = hcon->hdev->le_mtu;
749 else
750 conn->mtu = hcon->hdev->acl_mtu;
751
752 conn->src = &hcon->hdev->bdaddr;
753 conn->dst = &hcon->dst;
754
755 conn->feat_mask = 0;
756
757 spin_lock_init(&conn->lock);
758 rwlock_init(&conn->chan_list.lock);
759
760 if (hcon->type != LE_LINK)
761 setup_timer(&conn->info_timer, l2cap_info_timeout,
762 (unsigned long) conn);
763
764 conn->disc_reason = 0x13;
765
766 return conn;
767 }
768
769 static void l2cap_conn_del(struct hci_conn *hcon, int err)
770 {
771 struct l2cap_conn *conn = hcon->l2cap_data;
772 struct sock *sk;
773
774 if (!conn)
775 return;
776
777 BT_DBG("hcon %p conn %p, err %d", hcon, conn, err);
778
779 kfree_skb(conn->rx_skb);
780
781 /* Kill channels */
782 while ((sk = conn->chan_list.head)) {
783 bh_lock_sock(sk);
784 l2cap_chan_del(sk, err);
785 bh_unlock_sock(sk);
786 l2cap_sock_kill(sk);
787 }
788
789 if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT)
790 del_timer_sync(&conn->info_timer);
791
792 hcon->l2cap_data = NULL;
793 kfree(conn);
794 }
795
796 static inline void l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct sock *parent)
797 {
798 struct l2cap_chan_list *l = &conn->chan_list;
799 write_lock_bh(&l->lock);
800 __l2cap_chan_add(conn, sk, parent);
801 write_unlock_bh(&l->lock);
802 }
803
804 /* ---- Socket interface ---- */
805
806 /* Find socket with psm and source bdaddr.
807 * Returns closest match.
808 */
809 static struct sock *l2cap_get_sock_by_psm(int state, __le16 psm, bdaddr_t *src)
810 {
811 struct sock *sk = NULL, *sk1 = NULL;
812 struct hlist_node *node;
813
814 read_lock(&l2cap_sk_list.lock);
815
816 sk_for_each(sk, node, &l2cap_sk_list.head) {
817 if (state && sk->sk_state != state)
818 continue;
819
820 if (l2cap_pi(sk)->psm == psm) {
821 /* Exact match. */
822 if (!bacmp(&bt_sk(sk)->src, src))
823 break;
824
825 /* Closest match */
826 if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY))
827 sk1 = sk;
828 }
829 }
830
831 read_unlock(&l2cap_sk_list.lock);
832
833 return node ? sk : sk1;
834 }
835
836 int l2cap_do_connect(struct sock *sk)
837 {
838 bdaddr_t *src = &bt_sk(sk)->src;
839 bdaddr_t *dst = &bt_sk(sk)->dst;
840 struct l2cap_conn *conn;
841 struct hci_conn *hcon;
842 struct hci_dev *hdev;
843 __u8 auth_type;
844 int err;
845
846 BT_DBG("%s -> %s psm 0x%2.2x", batostr(src), batostr(dst),
847 l2cap_pi(sk)->psm);
848
849 hdev = hci_get_route(dst, src);
850 if (!hdev)
851 return -EHOSTUNREACH;
852
853 hci_dev_lock_bh(hdev);
854
855 auth_type = l2cap_get_auth_type(sk);
856
857 if (l2cap_pi(sk)->dcid == L2CAP_CID_LE_DATA)
858 hcon = hci_connect(hdev, LE_LINK, dst,
859 l2cap_pi(sk)->sec_level, auth_type);
860 else
861 hcon = hci_connect(hdev, ACL_LINK, dst,
862 l2cap_pi(sk)->sec_level, auth_type);
863
864 if (IS_ERR(hcon)) {
865 err = PTR_ERR(hcon);
866 goto done;
867 }
868
869 conn = l2cap_conn_add(hcon, 0);
870 if (!conn) {
871 hci_conn_put(hcon);
872 err = -ENOMEM;
873 goto done;
874 }
875
876 /* Update source addr of the socket */
877 bacpy(src, conn->src);
878
879 l2cap_chan_add(conn, sk, NULL);
880
881 sk->sk_state = BT_CONNECT;
882 l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
883
884 if (hcon->state == BT_CONNECTED) {
885 if (sk->sk_type != SOCK_SEQPACKET &&
886 sk->sk_type != SOCK_STREAM) {
887 l2cap_sock_clear_timer(sk);
888 if (l2cap_check_security(sk))
889 sk->sk_state = BT_CONNECTED;
890 } else
891 l2cap_do_start(sk);
892 }
893
894 err = 0;
895
896 done:
897 hci_dev_unlock_bh(hdev);
898 hci_dev_put(hdev);
899 return err;
900 }
901
902 int __l2cap_wait_ack(struct sock *sk)
903 {
904 DECLARE_WAITQUEUE(wait, current);
905 int err = 0;
906 int timeo = HZ/5;
907
908 add_wait_queue(sk_sleep(sk), &wait);
909 while ((l2cap_pi(sk)->unacked_frames > 0 && l2cap_pi(sk)->conn)) {
910 set_current_state(TASK_INTERRUPTIBLE);
911
912 if (!timeo)
913 timeo = HZ/5;
914
915 if (signal_pending(current)) {
916 err = sock_intr_errno(timeo);
917 break;
918 }
919
920 release_sock(sk);
921 timeo = schedule_timeout(timeo);
922 lock_sock(sk);
923
924 err = sock_error(sk);
925 if (err)
926 break;
927 }
928 set_current_state(TASK_RUNNING);
929 remove_wait_queue(sk_sleep(sk), &wait);
930 return err;
931 }
932
933 static void l2cap_monitor_timeout(unsigned long arg)
934 {
935 struct sock *sk = (void *) arg;
936
937 BT_DBG("sk %p", sk);
938
939 bh_lock_sock(sk);
940 if (l2cap_pi(sk)->retry_count >= l2cap_pi(sk)->remote_max_tx) {
941 l2cap_send_disconn_req(l2cap_pi(sk)->conn, sk, ECONNABORTED);
942 bh_unlock_sock(sk);
943 return;
944 }
945
946 l2cap_pi(sk)->retry_count++;
947 __mod_monitor_timer();
948
949 l2cap_send_rr_or_rnr(l2cap_pi(sk), L2CAP_CTRL_POLL);
950 bh_unlock_sock(sk);
951 }
952
953 static void l2cap_retrans_timeout(unsigned long arg)
954 {
955 struct sock *sk = (void *) arg;
956
957 BT_DBG("sk %p", sk);
958
959 bh_lock_sock(sk);
960 l2cap_pi(sk)->retry_count = 1;
961 __mod_monitor_timer();
962
963 l2cap_pi(sk)->conn_state |= L2CAP_CONN_WAIT_F;
964
965 l2cap_send_rr_or_rnr(l2cap_pi(sk), L2CAP_CTRL_POLL);
966 bh_unlock_sock(sk);
967 }
968
969 static void l2cap_drop_acked_frames(struct sock *sk)
970 {
971 struct sk_buff *skb;
972
973 while ((skb = skb_peek(TX_QUEUE(sk))) &&
974 l2cap_pi(sk)->unacked_frames) {
975 if (bt_cb(skb)->tx_seq == l2cap_pi(sk)->expected_ack_seq)
976 break;
977
978 skb = skb_dequeue(TX_QUEUE(sk));
979 kfree_skb(skb);
980
981 l2cap_pi(sk)->unacked_frames--;
982 }
983
984 if (!l2cap_pi(sk)->unacked_frames)
985 del_timer(&l2cap_pi(sk)->retrans_timer);
986 }
987
988 void l2cap_do_send(struct sock *sk, struct sk_buff *skb)
989 {
990 struct l2cap_pinfo *pi = l2cap_pi(sk);
991 struct hci_conn *hcon = pi->conn->hcon;
992 u16 flags;
993
994 BT_DBG("sk %p, skb %p len %d", sk, skb, skb->len);
995
996 if (!pi->flushable && lmp_no_flush_capable(hcon->hdev))
997 flags = ACL_START_NO_FLUSH;
998 else
999 flags = ACL_START;
1000
1001 hci_send_acl(hcon, skb, flags);
1002 }
1003
1004 void l2cap_streaming_send(struct sock *sk)
1005 {
1006 struct sk_buff *skb;
1007 struct l2cap_pinfo *pi = l2cap_pi(sk);
1008 u16 control, fcs;
1009
1010 while ((skb = skb_dequeue(TX_QUEUE(sk)))) {
1011 control = get_unaligned_le16(skb->data + L2CAP_HDR_SIZE);
1012 control |= pi->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT;
1013 put_unaligned_le16(control, skb->data + L2CAP_HDR_SIZE);
1014
1015 if (pi->fcs == L2CAP_FCS_CRC16) {
1016 fcs = crc16(0, (u8 *)skb->data, skb->len - 2);
1017 put_unaligned_le16(fcs, skb->data + skb->len - 2);
1018 }
1019
1020 l2cap_do_send(sk, skb);
1021
1022 pi->next_tx_seq = (pi->next_tx_seq + 1) % 64;
1023 }
1024 }
1025
1026 static void l2cap_retransmit_one_frame(struct sock *sk, u8 tx_seq)
1027 {
1028 struct l2cap_pinfo *pi = l2cap_pi(sk);
1029 struct sk_buff *skb, *tx_skb;
1030 u16 control, fcs;
1031
1032 skb = skb_peek(TX_QUEUE(sk));
1033 if (!skb)
1034 return;
1035
1036 do {
1037 if (bt_cb(skb)->tx_seq == tx_seq)
1038 break;
1039
1040 if (skb_queue_is_last(TX_QUEUE(sk), skb))
1041 return;
1042
1043 } while ((skb = skb_queue_next(TX_QUEUE(sk), skb)));
1044
1045 if (pi->remote_max_tx &&
1046 bt_cb(skb)->retries == pi->remote_max_tx) {
1047 l2cap_send_disconn_req(pi->conn, sk, ECONNABORTED);
1048 return;
1049 }
1050
1051 tx_skb = skb_clone(skb, GFP_ATOMIC);
1052 bt_cb(skb)->retries++;
1053 control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE);
1054
1055 if (pi->conn_state & L2CAP_CONN_SEND_FBIT) {
1056 control |= L2CAP_CTRL_FINAL;
1057 pi->conn_state &= ~L2CAP_CONN_SEND_FBIT;
1058 }
1059
1060 control |= (pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT)
1061 | (tx_seq << L2CAP_CTRL_TXSEQ_SHIFT);
1062
1063 put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE);
1064
1065 if (pi->fcs == L2CAP_FCS_CRC16) {
1066 fcs = crc16(0, (u8 *)tx_skb->data, tx_skb->len - 2);
1067 put_unaligned_le16(fcs, tx_skb->data + tx_skb->len - 2);
1068 }
1069
1070 l2cap_do_send(sk, tx_skb);
1071 }
1072
1073 int l2cap_ertm_send(struct sock *sk)
1074 {
1075 struct sk_buff *skb, *tx_skb;
1076 struct l2cap_pinfo *pi = l2cap_pi(sk);
1077 u16 control, fcs;
1078 int nsent = 0;
1079
1080 if (sk->sk_state != BT_CONNECTED)
1081 return -ENOTCONN;
1082
1083 while ((skb = sk->sk_send_head) && (!l2cap_tx_window_full(sk))) {
1084
1085 if (pi->remote_max_tx &&
1086 bt_cb(skb)->retries == pi->remote_max_tx) {
1087 l2cap_send_disconn_req(pi->conn, sk, ECONNABORTED);
1088 break;
1089 }
1090
1091 tx_skb = skb_clone(skb, GFP_ATOMIC);
1092
1093 bt_cb(skb)->retries++;
1094
1095 control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE);
1096 control &= L2CAP_CTRL_SAR;
1097
1098 if (pi->conn_state & L2CAP_CONN_SEND_FBIT) {
1099 control |= L2CAP_CTRL_FINAL;
1100 pi->conn_state &= ~L2CAP_CONN_SEND_FBIT;
1101 }
1102 control |= (pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT)
1103 | (pi->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT);
1104 put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE);
1105
1106
1107 if (pi->fcs == L2CAP_FCS_CRC16) {
1108 fcs = crc16(0, (u8 *)skb->data, tx_skb->len - 2);
1109 put_unaligned_le16(fcs, skb->data + tx_skb->len - 2);
1110 }
1111
1112 l2cap_do_send(sk, tx_skb);
1113
1114 __mod_retrans_timer();
1115
1116 bt_cb(skb)->tx_seq = pi->next_tx_seq;
1117 pi->next_tx_seq = (pi->next_tx_seq + 1) % 64;
1118
1119 if (bt_cb(skb)->retries == 1)
1120 pi->unacked_frames++;
1121
1122 pi->frames_sent++;
1123
1124 if (skb_queue_is_last(TX_QUEUE(sk), skb))
1125 sk->sk_send_head = NULL;
1126 else
1127 sk->sk_send_head = skb_queue_next(TX_QUEUE(sk), skb);
1128
1129 nsent++;
1130 }
1131
1132 return nsent;
1133 }
1134
1135 static int l2cap_retransmit_frames(struct sock *sk)
1136 {
1137 struct l2cap_pinfo *pi = l2cap_pi(sk);
1138 int ret;
1139
1140 if (!skb_queue_empty(TX_QUEUE(sk)))
1141 sk->sk_send_head = TX_QUEUE(sk)->next;
1142
1143 pi->next_tx_seq = pi->expected_ack_seq;
1144 ret = l2cap_ertm_send(sk);
1145 return ret;
1146 }
1147
1148 static void l2cap_send_ack(struct l2cap_pinfo *pi)
1149 {
1150 struct sock *sk = (struct sock *)pi;
1151 u16 control = 0;
1152
1153 control |= pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
1154
1155 if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
1156 control |= L2CAP_SUPER_RCV_NOT_READY;
1157 pi->conn_state |= L2CAP_CONN_RNR_SENT;
1158 l2cap_send_sframe(pi, control);
1159 return;
1160 }
1161
1162 if (l2cap_ertm_send(sk) > 0)
1163 return;
1164
1165 control |= L2CAP_SUPER_RCV_READY;
1166 l2cap_send_sframe(pi, control);
1167 }
1168
1169 static void l2cap_send_srejtail(struct sock *sk)
1170 {
1171 struct srej_list *tail;
1172 u16 control;
1173
1174 control = L2CAP_SUPER_SELECT_REJECT;
1175 control |= L2CAP_CTRL_FINAL;
1176
1177 tail = list_entry(SREJ_LIST(sk)->prev, struct srej_list, list);
1178 control |= tail->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
1179
1180 l2cap_send_sframe(l2cap_pi(sk), control);
1181 }
1182
1183 static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, int len, int count, struct sk_buff *skb)
1184 {
1185 struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1186 struct sk_buff **frag;
1187 int err, sent = 0;
1188
1189 if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count))
1190 return -EFAULT;
1191
1192 sent += count;
1193 len -= count;
1194
1195 /* Continuation fragments (no L2CAP header) */
1196 frag = &skb_shinfo(skb)->frag_list;
1197 while (len) {
1198 count = min_t(unsigned int, conn->mtu, len);
1199
1200 *frag = bt_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err);
1201 if (!*frag)
1202 return err;
1203 if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count))
1204 return -EFAULT;
1205
1206 sent += count;
1207 len -= count;
1208
1209 frag = &(*frag)->next;
1210 }
1211
1212 return sent;
1213 }
1214
1215 struct sk_buff *l2cap_create_connless_pdu(struct sock *sk, struct msghdr *msg, size_t len)
1216 {
1217 struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1218 struct sk_buff *skb;
1219 int err, count, hlen = L2CAP_HDR_SIZE + 2;
1220 struct l2cap_hdr *lh;
1221
1222 BT_DBG("sk %p len %d", sk, (int)len);
1223
1224 count = min_t(unsigned int, (conn->mtu - hlen), len);
1225 skb = bt_skb_send_alloc(sk, count + hlen,
1226 msg->msg_flags & MSG_DONTWAIT, &err);
1227 if (!skb)
1228 return ERR_PTR(err);
1229
1230 /* Create L2CAP header */
1231 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1232 lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
1233 lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1234 put_unaligned_le16(l2cap_pi(sk)->psm, skb_put(skb, 2));
1235
1236 err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1237 if (unlikely(err < 0)) {
1238 kfree_skb(skb);
1239 return ERR_PTR(err);
1240 }
1241 return skb;
1242 }
1243
1244 struct sk_buff *l2cap_create_basic_pdu(struct sock *sk, struct msghdr *msg, size_t len)
1245 {
1246 struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1247 struct sk_buff *skb;
1248 int err, count, hlen = L2CAP_HDR_SIZE;
1249 struct l2cap_hdr *lh;
1250
1251 BT_DBG("sk %p len %d", sk, (int)len);
1252
1253 count = min_t(unsigned int, (conn->mtu - hlen), len);
1254 skb = bt_skb_send_alloc(sk, count + hlen,
1255 msg->msg_flags & MSG_DONTWAIT, &err);
1256 if (!skb)
1257 return ERR_PTR(err);
1258
1259 /* Create L2CAP header */
1260 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1261 lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
1262 lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1263
1264 err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1265 if (unlikely(err < 0)) {
1266 kfree_skb(skb);
1267 return ERR_PTR(err);
1268 }
1269 return skb;
1270 }
1271
1272 struct sk_buff *l2cap_create_iframe_pdu(struct sock *sk, struct msghdr *msg, size_t len, u16 control, u16 sdulen)
1273 {
1274 struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1275 struct sk_buff *skb;
1276 int err, count, hlen = L2CAP_HDR_SIZE + 2;
1277 struct l2cap_hdr *lh;
1278
1279 BT_DBG("sk %p len %d", sk, (int)len);
1280
1281 if (!conn)
1282 return ERR_PTR(-ENOTCONN);
1283
1284 if (sdulen)
1285 hlen += 2;
1286
1287 if (l2cap_pi(sk)->fcs == L2CAP_FCS_CRC16)
1288 hlen += 2;
1289
1290 count = min_t(unsigned int, (conn->mtu - hlen), len);
1291 skb = bt_skb_send_alloc(sk, count + hlen,
1292 msg->msg_flags & MSG_DONTWAIT, &err);
1293 if (!skb)
1294 return ERR_PTR(err);
1295
1296 /* Create L2CAP header */
1297 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1298 lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
1299 lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1300 put_unaligned_le16(control, skb_put(skb, 2));
1301 if (sdulen)
1302 put_unaligned_le16(sdulen, skb_put(skb, 2));
1303
1304 err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1305 if (unlikely(err < 0)) {
1306 kfree_skb(skb);
1307 return ERR_PTR(err);
1308 }
1309
1310 if (l2cap_pi(sk)->fcs == L2CAP_FCS_CRC16)
1311 put_unaligned_le16(0, skb_put(skb, 2));
1312
1313 bt_cb(skb)->retries = 0;
1314 return skb;
1315 }
1316
1317 int l2cap_sar_segment_sdu(struct sock *sk, struct msghdr *msg, size_t len)
1318 {
1319 struct l2cap_pinfo *pi = l2cap_pi(sk);
1320 struct sk_buff *skb;
1321 struct sk_buff_head sar_queue;
1322 u16 control;
1323 size_t size = 0;
1324
1325 skb_queue_head_init(&sar_queue);
1326 control = L2CAP_SDU_START;
1327 skb = l2cap_create_iframe_pdu(sk, msg, pi->remote_mps, control, len);
1328 if (IS_ERR(skb))
1329 return PTR_ERR(skb);
1330
1331 __skb_queue_tail(&sar_queue, skb);
1332 len -= pi->remote_mps;
1333 size += pi->remote_mps;
1334
1335 while (len > 0) {
1336 size_t buflen;
1337
1338 if (len > pi->remote_mps) {
1339 control = L2CAP_SDU_CONTINUE;
1340 buflen = pi->remote_mps;
1341 } else {
1342 control = L2CAP_SDU_END;
1343 buflen = len;
1344 }
1345
1346 skb = l2cap_create_iframe_pdu(sk, msg, buflen, control, 0);
1347 if (IS_ERR(skb)) {
1348 skb_queue_purge(&sar_queue);
1349 return PTR_ERR(skb);
1350 }
1351
1352 __skb_queue_tail(&sar_queue, skb);
1353 len -= buflen;
1354 size += buflen;
1355 }
1356 skb_queue_splice_tail(&sar_queue, TX_QUEUE(sk));
1357 if (sk->sk_send_head == NULL)
1358 sk->sk_send_head = sar_queue.next;
1359
1360 return size;
1361 }
1362
1363 static void l2cap_chan_ready(struct sock *sk)
1364 {
1365 struct sock *parent = bt_sk(sk)->parent;
1366
1367 BT_DBG("sk %p, parent %p", sk, parent);
1368
1369 l2cap_pi(sk)->conf_state = 0;
1370 l2cap_sock_clear_timer(sk);
1371
1372 if (!parent) {
1373 /* Outgoing channel.
1374 * Wake up socket sleeping on connect.
1375 */
1376 sk->sk_state = BT_CONNECTED;
1377 sk->sk_state_change(sk);
1378 } else {
1379 /* Incoming channel.
1380 * Wake up socket sleeping on accept.
1381 */
1382 parent->sk_data_ready(parent, 0);
1383 }
1384 }
1385
1386 /* Copy frame to all raw sockets on that connection */
1387 static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb)
1388 {
1389 struct l2cap_chan_list *l = &conn->chan_list;
1390 struct sk_buff *nskb;
1391 struct sock *sk;
1392
1393 BT_DBG("conn %p", conn);
1394
1395 read_lock(&l->lock);
1396 for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
1397 if (sk->sk_type != SOCK_RAW)
1398 continue;
1399
1400 /* Don't send frame to the socket it came from */
1401 if (skb->sk == sk)
1402 continue;
1403 nskb = skb_clone(skb, GFP_ATOMIC);
1404 if (!nskb)
1405 continue;
1406
1407 if (sock_queue_rcv_skb(sk, nskb))
1408 kfree_skb(nskb);
1409 }
1410 read_unlock(&l->lock);
1411 }
1412
1413 /* ---- L2CAP signalling commands ---- */
1414 static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
1415 u8 code, u8 ident, u16 dlen, void *data)
1416 {
1417 struct sk_buff *skb, **frag;
1418 struct l2cap_cmd_hdr *cmd;
1419 struct l2cap_hdr *lh;
1420 int len, count;
1421
1422 BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %d",
1423 conn, code, ident, dlen);
1424
1425 len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen;
1426 count = min_t(unsigned int, conn->mtu, len);
1427
1428 skb = bt_skb_alloc(count, GFP_ATOMIC);
1429 if (!skb)
1430 return NULL;
1431
1432 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1433 lh->len = cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen);
1434
1435 if (conn->hcon->type == LE_LINK)
1436 lh->cid = cpu_to_le16(L2CAP_CID_LE_SIGNALING);
1437 else
1438 lh->cid = cpu_to_le16(L2CAP_CID_SIGNALING);
1439
1440 cmd = (struct l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE);
1441 cmd->code = code;
1442 cmd->ident = ident;
1443 cmd->len = cpu_to_le16(dlen);
1444
1445 if (dlen) {
1446 count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE;
1447 memcpy(skb_put(skb, count), data, count);
1448 data += count;
1449 }
1450
1451 len -= skb->len;
1452
1453 /* Continuation fragments (no L2CAP header) */
1454 frag = &skb_shinfo(skb)->frag_list;
1455 while (len) {
1456 count = min_t(unsigned int, conn->mtu, len);
1457
1458 *frag = bt_skb_alloc(count, GFP_ATOMIC);
1459 if (!*frag)
1460 goto fail;
1461
1462 memcpy(skb_put(*frag, count), data, count);
1463
1464 len -= count;
1465 data += count;
1466
1467 frag = &(*frag)->next;
1468 }
1469
1470 return skb;
1471
1472 fail:
1473 kfree_skb(skb);
1474 return NULL;
1475 }
1476
1477 static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, unsigned long *val)
1478 {
1479 struct l2cap_conf_opt *opt = *ptr;
1480 int len;
1481
1482 len = L2CAP_CONF_OPT_SIZE + opt->len;
1483 *ptr += len;
1484
1485 *type = opt->type;
1486 *olen = opt->len;
1487
1488 switch (opt->len) {
1489 case 1:
1490 *val = *((u8 *) opt->val);
1491 break;
1492
1493 case 2:
1494 *val = get_unaligned_le16(opt->val);
1495 break;
1496
1497 case 4:
1498 *val = get_unaligned_le32(opt->val);
1499 break;
1500
1501 default:
1502 *val = (unsigned long) opt->val;
1503 break;
1504 }
1505
1506 BT_DBG("type 0x%2.2x len %d val 0x%lx", *type, opt->len, *val);
1507 return len;
1508 }
1509
1510 static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val)
1511 {
1512 struct l2cap_conf_opt *opt = *ptr;
1513
1514 BT_DBG("type 0x%2.2x len %d val 0x%lx", type, len, val);
1515
1516 opt->type = type;
1517 opt->len = len;
1518
1519 switch (len) {
1520 case 1:
1521 *((u8 *) opt->val) = val;
1522 break;
1523
1524 case 2:
1525 put_unaligned_le16(val, opt->val);
1526 break;
1527
1528 case 4:
1529 put_unaligned_le32(val, opt->val);
1530 break;
1531
1532 default:
1533 memcpy(opt->val, (void *) val, len);
1534 break;
1535 }
1536
1537 *ptr += L2CAP_CONF_OPT_SIZE + len;
1538 }
1539
1540 static void l2cap_ack_timeout(unsigned long arg)
1541 {
1542 struct sock *sk = (void *) arg;
1543
1544 bh_lock_sock(sk);
1545 l2cap_send_ack(l2cap_pi(sk));
1546 bh_unlock_sock(sk);
1547 }
1548
1549 static inline void l2cap_ertm_init(struct sock *sk)
1550 {
1551 l2cap_pi(sk)->expected_ack_seq = 0;
1552 l2cap_pi(sk)->unacked_frames = 0;
1553 l2cap_pi(sk)->buffer_seq = 0;
1554 l2cap_pi(sk)->num_acked = 0;
1555 l2cap_pi(sk)->frames_sent = 0;
1556
1557 setup_timer(&l2cap_pi(sk)->retrans_timer,
1558 l2cap_retrans_timeout, (unsigned long) sk);
1559 setup_timer(&l2cap_pi(sk)->monitor_timer,
1560 l2cap_monitor_timeout, (unsigned long) sk);
1561 setup_timer(&l2cap_pi(sk)->ack_timer,
1562 l2cap_ack_timeout, (unsigned long) sk);
1563
1564 __skb_queue_head_init(SREJ_QUEUE(sk));
1565 __skb_queue_head_init(BUSY_QUEUE(sk));
1566
1567 INIT_WORK(&l2cap_pi(sk)->busy_work, l2cap_busy_work);
1568
1569 sk->sk_backlog_rcv = l2cap_ertm_data_rcv;
1570 }
1571
1572 static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask)
1573 {
1574 switch (mode) {
1575 case L2CAP_MODE_STREAMING:
1576 case L2CAP_MODE_ERTM:
1577 if (l2cap_mode_supported(mode, remote_feat_mask))
1578 return mode;
1579 /* fall through */
1580 default:
1581 return L2CAP_MODE_BASIC;
1582 }
1583 }
1584
1585 int l2cap_build_conf_req(struct sock *sk, void *data)
1586 {
1587 struct l2cap_pinfo *pi = l2cap_pi(sk);
1588 struct l2cap_conf_req *req = data;
1589 struct l2cap_conf_rfc rfc = { .mode = pi->mode };
1590 void *ptr = req->data;
1591
1592 BT_DBG("sk %p", sk);
1593
1594 if (pi->num_conf_req || pi->num_conf_rsp)
1595 goto done;
1596
1597 switch (pi->mode) {
1598 case L2CAP_MODE_STREAMING:
1599 case L2CAP_MODE_ERTM:
1600 if (pi->conf_state & L2CAP_CONF_STATE2_DEVICE)
1601 break;
1602
1603 /* fall through */
1604 default:
1605 pi->mode = l2cap_select_mode(rfc.mode, pi->conn->feat_mask);
1606 break;
1607 }
1608
1609 done:
1610 if (pi->imtu != L2CAP_DEFAULT_MTU)
1611 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->imtu);
1612
1613 switch (pi->mode) {
1614 case L2CAP_MODE_BASIC:
1615 if (!(pi->conn->feat_mask & L2CAP_FEAT_ERTM) &&
1616 !(pi->conn->feat_mask & L2CAP_FEAT_STREAMING))
1617 break;
1618
1619 rfc.mode = L2CAP_MODE_BASIC;
1620 rfc.txwin_size = 0;
1621 rfc.max_transmit = 0;
1622 rfc.retrans_timeout = 0;
1623 rfc.monitor_timeout = 0;
1624 rfc.max_pdu_size = 0;
1625
1626 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
1627 (unsigned long) &rfc);
1628 break;
1629
1630 case L2CAP_MODE_ERTM:
1631 rfc.mode = L2CAP_MODE_ERTM;
1632 rfc.txwin_size = pi->tx_win;
1633 rfc.max_transmit = pi->max_tx;
1634 rfc.retrans_timeout = 0;
1635 rfc.monitor_timeout = 0;
1636 rfc.max_pdu_size = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE);
1637 if (L2CAP_DEFAULT_MAX_PDU_SIZE > pi->conn->mtu - 10)
1638 rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10);
1639
1640 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
1641 (unsigned long) &rfc);
1642
1643 if (!(pi->conn->feat_mask & L2CAP_FEAT_FCS))
1644 break;
1645
1646 if (pi->fcs == L2CAP_FCS_NONE ||
1647 pi->conf_state & L2CAP_CONF_NO_FCS_RECV) {
1648 pi->fcs = L2CAP_FCS_NONE;
1649 l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, pi->fcs);
1650 }
1651 break;
1652
1653 case L2CAP_MODE_STREAMING:
1654 rfc.mode = L2CAP_MODE_STREAMING;
1655 rfc.txwin_size = 0;
1656 rfc.max_transmit = 0;
1657 rfc.retrans_timeout = 0;
1658 rfc.monitor_timeout = 0;
1659 rfc.max_pdu_size = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE);
1660 if (L2CAP_DEFAULT_MAX_PDU_SIZE > pi->conn->mtu - 10)
1661 rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10);
1662
1663 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
1664 (unsigned long) &rfc);
1665
1666 if (!(pi->conn->feat_mask & L2CAP_FEAT_FCS))
1667 break;
1668
1669 if (pi->fcs == L2CAP_FCS_NONE ||
1670 pi->conf_state & L2CAP_CONF_NO_FCS_RECV) {
1671 pi->fcs = L2CAP_FCS_NONE;
1672 l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, pi->fcs);
1673 }
1674 break;
1675 }
1676
1677 req->dcid = cpu_to_le16(pi->dcid);
1678 req->flags = cpu_to_le16(0);
1679
1680 return ptr - data;
1681 }
1682
1683 static int l2cap_parse_conf_req(struct sock *sk, void *data)
1684 {
1685 struct l2cap_pinfo *pi = l2cap_pi(sk);
1686 struct l2cap_conf_rsp *rsp = data;
1687 void *ptr = rsp->data;
1688 void *req = pi->conf_req;
1689 int len = pi->conf_len;
1690 int type, hint, olen;
1691 unsigned long val;
1692 struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
1693 u16 mtu = L2CAP_DEFAULT_MTU;
1694 u16 result = L2CAP_CONF_SUCCESS;
1695
1696 BT_DBG("sk %p", sk);
1697
1698 while (len >= L2CAP_CONF_OPT_SIZE) {
1699 len -= l2cap_get_conf_opt(&req, &type, &olen, &val);
1700
1701 hint = type & L2CAP_CONF_HINT;
1702 type &= L2CAP_CONF_MASK;
1703
1704 switch (type) {
1705 case L2CAP_CONF_MTU:
1706 mtu = val;
1707 break;
1708
1709 case L2CAP_CONF_FLUSH_TO:
1710 pi->flush_to = val;
1711 break;
1712
1713 case L2CAP_CONF_QOS:
1714 break;
1715
1716 case L2CAP_CONF_RFC:
1717 if (olen == sizeof(rfc))
1718 memcpy(&rfc, (void *) val, olen);
1719 break;
1720
1721 case L2CAP_CONF_FCS:
1722 if (val == L2CAP_FCS_NONE)
1723 pi->conf_state |= L2CAP_CONF_NO_FCS_RECV;
1724
1725 break;
1726
1727 default:
1728 if (hint)
1729 break;
1730
1731 result = L2CAP_CONF_UNKNOWN;
1732 *((u8 *) ptr++) = type;
1733 break;
1734 }
1735 }
1736
1737 if (pi->num_conf_rsp || pi->num_conf_req > 1)
1738 goto done;
1739
1740 switch (pi->mode) {
1741 case L2CAP_MODE_STREAMING:
1742 case L2CAP_MODE_ERTM:
1743 if (!(pi->conf_state & L2CAP_CONF_STATE2_DEVICE)) {
1744 pi->mode = l2cap_select_mode(rfc.mode,
1745 pi->conn->feat_mask);
1746 break;
1747 }
1748
1749 if (pi->mode != rfc.mode)
1750 return -ECONNREFUSED;
1751
1752 break;
1753 }
1754
1755 done:
1756 if (pi->mode != rfc.mode) {
1757 result = L2CAP_CONF_UNACCEPT;
1758 rfc.mode = pi->mode;
1759
1760 if (pi->num_conf_rsp == 1)
1761 return -ECONNREFUSED;
1762
1763 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
1764 sizeof(rfc), (unsigned long) &rfc);
1765 }
1766
1767
1768 if (result == L2CAP_CONF_SUCCESS) {
1769 /* Configure output options and let the other side know
1770 * which ones we don't like. */
1771
1772 if (mtu < L2CAP_DEFAULT_MIN_MTU)
1773 result = L2CAP_CONF_UNACCEPT;
1774 else {
1775 pi->omtu = mtu;
1776 pi->conf_state |= L2CAP_CONF_MTU_DONE;
1777 }
1778 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->omtu);
1779
1780 switch (rfc.mode) {
1781 case L2CAP_MODE_BASIC:
1782 pi->fcs = L2CAP_FCS_NONE;
1783 pi->conf_state |= L2CAP_CONF_MODE_DONE;
1784 break;
1785
1786 case L2CAP_MODE_ERTM:
1787 pi->remote_tx_win = rfc.txwin_size;
1788 pi->remote_max_tx = rfc.max_transmit;
1789
1790 if (le16_to_cpu(rfc.max_pdu_size) > pi->conn->mtu - 10)
1791 rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10);
1792
1793 pi->remote_mps = le16_to_cpu(rfc.max_pdu_size);
1794
1795 rfc.retrans_timeout =
1796 le16_to_cpu(L2CAP_DEFAULT_RETRANS_TO);
1797 rfc.monitor_timeout =
1798 le16_to_cpu(L2CAP_DEFAULT_MONITOR_TO);
1799
1800 pi->conf_state |= L2CAP_CONF_MODE_DONE;
1801
1802 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
1803 sizeof(rfc), (unsigned long) &rfc);
1804
1805 break;
1806
1807 case L2CAP_MODE_STREAMING:
1808 if (le16_to_cpu(rfc.max_pdu_size) > pi->conn->mtu - 10)
1809 rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10);
1810
1811 pi->remote_mps = le16_to_cpu(rfc.max_pdu_size);
1812
1813 pi->conf_state |= L2CAP_CONF_MODE_DONE;
1814
1815 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
1816 sizeof(rfc), (unsigned long) &rfc);
1817
1818 break;
1819
1820 default:
1821 result = L2CAP_CONF_UNACCEPT;
1822
1823 memset(&rfc, 0, sizeof(rfc));
1824 rfc.mode = pi->mode;
1825 }
1826
1827 if (result == L2CAP_CONF_SUCCESS)
1828 pi->conf_state |= L2CAP_CONF_OUTPUT_DONE;
1829 }
1830 rsp->scid = cpu_to_le16(pi->dcid);
1831 rsp->result = cpu_to_le16(result);
1832 rsp->flags = cpu_to_le16(0x0000);
1833
1834 return ptr - data;
1835 }
1836
1837 static int l2cap_parse_conf_rsp(struct sock *sk, void *rsp, int len, void *data, u16 *result)
1838 {
1839 struct l2cap_pinfo *pi = l2cap_pi(sk);
1840 struct l2cap_conf_req *req = data;
1841 void *ptr = req->data;
1842 int type, olen;
1843 unsigned long val;
1844 struct l2cap_conf_rfc rfc;
1845
1846 BT_DBG("sk %p, rsp %p, len %d, req %p", sk, rsp, len, data);
1847
1848 while (len >= L2CAP_CONF_OPT_SIZE) {
1849 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
1850
1851 switch (type) {
1852 case L2CAP_CONF_MTU:
1853 if (val < L2CAP_DEFAULT_MIN_MTU) {
1854 *result = L2CAP_CONF_UNACCEPT;
1855 pi->imtu = L2CAP_DEFAULT_MIN_MTU;
1856 } else
1857 pi->imtu = val;
1858 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->imtu);
1859 break;
1860
1861 case L2CAP_CONF_FLUSH_TO:
1862 pi->flush_to = val;
1863 l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
1864 2, pi->flush_to);
1865 break;
1866
1867 case L2CAP_CONF_RFC:
1868 if (olen == sizeof(rfc))
1869 memcpy(&rfc, (void *)val, olen);
1870
1871 if ((pi->conf_state & L2CAP_CONF_STATE2_DEVICE) &&
1872 rfc.mode != pi->mode)
1873 return -ECONNREFUSED;
1874
1875 pi->fcs = 0;
1876
1877 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
1878 sizeof(rfc), (unsigned long) &rfc);
1879 break;
1880 }
1881 }
1882
1883 if (pi->mode == L2CAP_MODE_BASIC && pi->mode != rfc.mode)
1884 return -ECONNREFUSED;
1885
1886 pi->mode = rfc.mode;
1887
1888 if (*result == L2CAP_CONF_SUCCESS) {
1889 switch (rfc.mode) {
1890 case L2CAP_MODE_ERTM:
1891 pi->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
1892 pi->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
1893 pi->mps = le16_to_cpu(rfc.max_pdu_size);
1894 break;
1895 case L2CAP_MODE_STREAMING:
1896 pi->mps = le16_to_cpu(rfc.max_pdu_size);
1897 }
1898 }
1899
1900 req->dcid = cpu_to_le16(pi->dcid);
1901 req->flags = cpu_to_le16(0x0000);
1902
1903 return ptr - data;
1904 }
1905
1906 static int l2cap_build_conf_rsp(struct sock *sk, void *data, u16 result, u16 flags)
1907 {
1908 struct l2cap_conf_rsp *rsp = data;
1909 void *ptr = rsp->data;
1910
1911 BT_DBG("sk %p", sk);
1912
1913 rsp->scid = cpu_to_le16(l2cap_pi(sk)->dcid);
1914 rsp->result = cpu_to_le16(result);
1915 rsp->flags = cpu_to_le16(flags);
1916
1917 return ptr - data;
1918 }
1919
1920 static void l2cap_conf_rfc_get(struct sock *sk, void *rsp, int len)
1921 {
1922 struct l2cap_pinfo *pi = l2cap_pi(sk);
1923 int type, olen;
1924 unsigned long val;
1925 struct l2cap_conf_rfc rfc;
1926
1927 BT_DBG("sk %p, rsp %p, len %d", sk, rsp, len);
1928
1929 if ((pi->mode != L2CAP_MODE_ERTM) && (pi->mode != L2CAP_MODE_STREAMING))
1930 return;
1931
1932 while (len >= L2CAP_CONF_OPT_SIZE) {
1933 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
1934
1935 switch (type) {
1936 case L2CAP_CONF_RFC:
1937 if (olen == sizeof(rfc))
1938 memcpy(&rfc, (void *)val, olen);
1939 goto done;
1940 }
1941 }
1942
1943 done:
1944 switch (rfc.mode) {
1945 case L2CAP_MODE_ERTM:
1946 pi->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
1947 pi->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
1948 pi->mps = le16_to_cpu(rfc.max_pdu_size);
1949 break;
1950 case L2CAP_MODE_STREAMING:
1951 pi->mps = le16_to_cpu(rfc.max_pdu_size);
1952 }
1953 }
1954
1955 static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1956 {
1957 struct l2cap_cmd_rej *rej = (struct l2cap_cmd_rej *) data;
1958
1959 if (rej->reason != 0x0000)
1960 return 0;
1961
1962 if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
1963 cmd->ident == conn->info_ident) {
1964 del_timer(&conn->info_timer);
1965
1966 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
1967 conn->info_ident = 0;
1968
1969 l2cap_conn_start(conn);
1970 }
1971
1972 return 0;
1973 }
1974
1975 static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1976 {
1977 struct l2cap_chan_list *list = &conn->chan_list;
1978 struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
1979 struct l2cap_conn_rsp rsp;
1980 struct sock *parent, *sk = NULL;
1981 int result, status = L2CAP_CS_NO_INFO;
1982
1983 u16 dcid = 0, scid = __le16_to_cpu(req->scid);
1984 __le16 psm = req->psm;
1985
1986 BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid);
1987
1988 /* Check if we have socket listening on psm */
1989 parent = l2cap_get_sock_by_psm(BT_LISTEN, psm, conn->src);
1990 if (!parent) {
1991 result = L2CAP_CR_BAD_PSM;
1992 goto sendresp;
1993 }
1994
1995 bh_lock_sock(parent);
1996
1997 /* Check if the ACL is secure enough (if not SDP) */
1998 if (psm != cpu_to_le16(0x0001) &&
1999 !hci_conn_check_link_mode(conn->hcon)) {
2000 conn->disc_reason = 0x05;
2001 result = L2CAP_CR_SEC_BLOCK;
2002 goto response;
2003 }
2004
2005 result = L2CAP_CR_NO_MEM;
2006
2007 /* Check for backlog size */
2008 if (sk_acceptq_is_full(parent)) {
2009 BT_DBG("backlog full %d", parent->sk_ack_backlog);
2010 goto response;
2011 }
2012
2013 sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP, GFP_ATOMIC);
2014 if (!sk)
2015 goto response;
2016
2017 write_lock_bh(&list->lock);
2018
2019 /* Check if we already have channel with that dcid */
2020 if (__l2cap_get_chan_by_dcid(list, scid)) {
2021 write_unlock_bh(&list->lock);
2022 sock_set_flag(sk, SOCK_ZAPPED);
2023 l2cap_sock_kill(sk);
2024 goto response;
2025 }
2026
2027 hci_conn_hold(conn->hcon);
2028
2029 l2cap_sock_init(sk, parent);
2030 bacpy(&bt_sk(sk)->src, conn->src);
2031 bacpy(&bt_sk(sk)->dst, conn->dst);
2032 l2cap_pi(sk)->psm = psm;
2033 l2cap_pi(sk)->dcid = scid;
2034
2035 __l2cap_chan_add(conn, sk, parent);
2036 dcid = l2cap_pi(sk)->scid;
2037
2038 l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
2039
2040 l2cap_pi(sk)->ident = cmd->ident;
2041
2042 if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
2043 if (l2cap_check_security(sk)) {
2044 if (bt_sk(sk)->defer_setup) {
2045 sk->sk_state = BT_CONNECT2;
2046 result = L2CAP_CR_PEND;
2047 status = L2CAP_CS_AUTHOR_PEND;
2048 parent->sk_data_ready(parent, 0);
2049 } else {
2050 sk->sk_state = BT_CONFIG;
2051 result = L2CAP_CR_SUCCESS;
2052 status = L2CAP_CS_NO_INFO;
2053 }
2054 } else {
2055 sk->sk_state = BT_CONNECT2;
2056 result = L2CAP_CR_PEND;
2057 status = L2CAP_CS_AUTHEN_PEND;
2058 }
2059 } else {
2060 sk->sk_state = BT_CONNECT2;
2061 result = L2CAP_CR_PEND;
2062 status = L2CAP_CS_NO_INFO;
2063 }
2064
2065 write_unlock_bh(&list->lock);
2066
2067 response:
2068 bh_unlock_sock(parent);
2069
2070 sendresp:
2071 rsp.scid = cpu_to_le16(scid);
2072 rsp.dcid = cpu_to_le16(dcid);
2073 rsp.result = cpu_to_le16(result);
2074 rsp.status = cpu_to_le16(status);
2075 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
2076
2077 if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
2078 struct l2cap_info_req info;
2079 info.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
2080
2081 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
2082 conn->info_ident = l2cap_get_ident(conn);
2083
2084 mod_timer(&conn->info_timer, jiffies +
2085 msecs_to_jiffies(L2CAP_INFO_TIMEOUT));
2086
2087 l2cap_send_cmd(conn, conn->info_ident,
2088 L2CAP_INFO_REQ, sizeof(info), &info);
2089 }
2090
2091 if (sk && !(l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT) &&
2092 result == L2CAP_CR_SUCCESS) {
2093 u8 buf[128];
2094 l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
2095 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2096 l2cap_build_conf_req(sk, buf), buf);
2097 l2cap_pi(sk)->num_conf_req++;
2098 }
2099
2100 return 0;
2101 }
2102
2103 static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2104 {
2105 struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
2106 u16 scid, dcid, result, status;
2107 struct sock *sk;
2108 u8 req[128];
2109
2110 scid = __le16_to_cpu(rsp->scid);
2111 dcid = __le16_to_cpu(rsp->dcid);
2112 result = __le16_to_cpu(rsp->result);
2113 status = __le16_to_cpu(rsp->status);
2114
2115 BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", dcid, scid, result, status);
2116
2117 if (scid) {
2118 sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
2119 if (!sk)
2120 return -EFAULT;
2121 } else {
2122 sk = l2cap_get_chan_by_ident(&conn->chan_list, cmd->ident);
2123 if (!sk)
2124 return -EFAULT;
2125 }
2126
2127 switch (result) {
2128 case L2CAP_CR_SUCCESS:
2129 sk->sk_state = BT_CONFIG;
2130 l2cap_pi(sk)->ident = 0;
2131 l2cap_pi(sk)->dcid = dcid;
2132 l2cap_pi(sk)->conf_state &= ~L2CAP_CONF_CONNECT_PEND;
2133
2134 if (l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT)
2135 break;
2136
2137 l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
2138
2139 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2140 l2cap_build_conf_req(sk, req), req);
2141 l2cap_pi(sk)->num_conf_req++;
2142 break;
2143
2144 case L2CAP_CR_PEND:
2145 l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND;
2146 break;
2147
2148 default:
2149 /* don't delete l2cap channel if sk is owned by user */
2150 if (sock_owned_by_user(sk)) {
2151 sk->sk_state = BT_DISCONN;
2152 l2cap_sock_clear_timer(sk);
2153 l2cap_sock_set_timer(sk, HZ / 5);
2154 break;
2155 }
2156
2157 l2cap_chan_del(sk, ECONNREFUSED);
2158 break;
2159 }
2160
2161 bh_unlock_sock(sk);
2162 return 0;
2163 }
2164
2165 static inline void set_default_fcs(struct l2cap_pinfo *pi)
2166 {
2167 /* FCS is enabled only in ERTM or streaming mode, if one or both
2168 * sides request it.
2169 */
2170 if (pi->mode != L2CAP_MODE_ERTM && pi->mode != L2CAP_MODE_STREAMING)
2171 pi->fcs = L2CAP_FCS_NONE;
2172 else if (!(pi->conf_state & L2CAP_CONF_NO_FCS_RECV))
2173 pi->fcs = L2CAP_FCS_CRC16;
2174 }
2175
2176 static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
2177 {
2178 struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
2179 u16 dcid, flags;
2180 u8 rsp[64];
2181 struct sock *sk;
2182 int len;
2183
2184 dcid = __le16_to_cpu(req->dcid);
2185 flags = __le16_to_cpu(req->flags);
2186
2187 BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
2188
2189 sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid);
2190 if (!sk)
2191 return -ENOENT;
2192
2193 if (sk->sk_state != BT_CONFIG) {
2194 struct l2cap_cmd_rej rej;
2195
2196 rej.reason = cpu_to_le16(0x0002);
2197 l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ,
2198 sizeof(rej), &rej);
2199 goto unlock;
2200 }
2201
2202 /* Reject if config buffer is too small. */
2203 len = cmd_len - sizeof(*req);
2204 if (l2cap_pi(sk)->conf_len + len > sizeof(l2cap_pi(sk)->conf_req)) {
2205 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2206 l2cap_build_conf_rsp(sk, rsp,
2207 L2CAP_CONF_REJECT, flags), rsp);
2208 goto unlock;
2209 }
2210
2211 /* Store config. */
2212 memcpy(l2cap_pi(sk)->conf_req + l2cap_pi(sk)->conf_len, req->data, len);
2213 l2cap_pi(sk)->conf_len += len;
2214
2215 if (flags & 0x0001) {
2216 /* Incomplete config. Send empty response. */
2217 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2218 l2cap_build_conf_rsp(sk, rsp,
2219 L2CAP_CONF_SUCCESS, 0x0001), rsp);
2220 goto unlock;
2221 }
2222
2223 /* Complete config. */
2224 len = l2cap_parse_conf_req(sk, rsp);
2225 if (len < 0) {
2226 l2cap_send_disconn_req(conn, sk, ECONNRESET);
2227 goto unlock;
2228 }
2229
2230 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
2231 l2cap_pi(sk)->num_conf_rsp++;
2232
2233 /* Reset config buffer. */
2234 l2cap_pi(sk)->conf_len = 0;
2235
2236 if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE))
2237 goto unlock;
2238
2239 if (l2cap_pi(sk)->conf_state & L2CAP_CONF_INPUT_DONE) {
2240 set_default_fcs(l2cap_pi(sk));
2241
2242 sk->sk_state = BT_CONNECTED;
2243
2244 l2cap_pi(sk)->next_tx_seq = 0;
2245 l2cap_pi(sk)->expected_tx_seq = 0;
2246 __skb_queue_head_init(TX_QUEUE(sk));
2247 if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM)
2248 l2cap_ertm_init(sk);
2249
2250 l2cap_chan_ready(sk);
2251 goto unlock;
2252 }
2253
2254 if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT)) {
2255 u8 buf[64];
2256 l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
2257 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2258 l2cap_build_conf_req(sk, buf), buf);
2259 l2cap_pi(sk)->num_conf_req++;
2260 }
2261
2262 unlock:
2263 bh_unlock_sock(sk);
2264 return 0;
2265 }
2266
2267 static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2268 {
2269 struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
2270 u16 scid, flags, result;
2271 struct sock *sk;
2272 int len = cmd->len - sizeof(*rsp);
2273
2274 scid = __le16_to_cpu(rsp->scid);
2275 flags = __le16_to_cpu(rsp->flags);
2276 result = __le16_to_cpu(rsp->result);
2277
2278 BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x",
2279 scid, flags, result);
2280
2281 sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
2282 if (!sk)
2283 return 0;
2284
2285 switch (result) {
2286 case L2CAP_CONF_SUCCESS:
2287 l2cap_conf_rfc_get(sk, rsp->data, len);
2288 break;
2289
2290 case L2CAP_CONF_UNACCEPT:
2291 if (l2cap_pi(sk)->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
2292 char req[64];
2293
2294 if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) {
2295 l2cap_send_disconn_req(conn, sk, ECONNRESET);
2296 goto done;
2297 }
2298
2299 /* throw out any old stored conf requests */
2300 result = L2CAP_CONF_SUCCESS;
2301 len = l2cap_parse_conf_rsp(sk, rsp->data,
2302 len, req, &result);
2303 if (len < 0) {
2304 l2cap_send_disconn_req(conn, sk, ECONNRESET);
2305 goto done;
2306 }
2307
2308 l2cap_send_cmd(conn, l2cap_get_ident(conn),
2309 L2CAP_CONF_REQ, len, req);
2310 l2cap_pi(sk)->num_conf_req++;
2311 if (result != L2CAP_CONF_SUCCESS)
2312 goto done;
2313 break;
2314 }
2315
2316 default:
2317 sk->sk_err = ECONNRESET;
2318 l2cap_sock_set_timer(sk, HZ * 5);
2319 l2cap_send_disconn_req(conn, sk, ECONNRESET);
2320 goto done;
2321 }
2322
2323 if (flags & 0x01)
2324 goto done;
2325
2326 l2cap_pi(sk)->conf_state |= L2CAP_CONF_INPUT_DONE;
2327
2328 if (l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE) {
2329 set_default_fcs(l2cap_pi(sk));
2330
2331 sk->sk_state = BT_CONNECTED;
2332 l2cap_pi(sk)->next_tx_seq = 0;
2333 l2cap_pi(sk)->expected_tx_seq = 0;
2334 __skb_queue_head_init(TX_QUEUE(sk));
2335 if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM)
2336 l2cap_ertm_init(sk);
2337
2338 l2cap_chan_ready(sk);
2339 }
2340
2341 done:
2342 bh_unlock_sock(sk);
2343 return 0;
2344 }
2345
2346 static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2347 {
2348 struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
2349 struct l2cap_disconn_rsp rsp;
2350 u16 dcid, scid;
2351 struct sock *sk;
2352
2353 scid = __le16_to_cpu(req->scid);
2354 dcid = __le16_to_cpu(req->dcid);
2355
2356 BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
2357
2358 sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid);
2359 if (!sk)
2360 return 0;
2361
2362 rsp.dcid = cpu_to_le16(l2cap_pi(sk)->scid);
2363 rsp.scid = cpu_to_le16(l2cap_pi(sk)->dcid);
2364 l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
2365
2366 sk->sk_shutdown = SHUTDOWN_MASK;
2367
2368 /* don't delete l2cap channel if sk is owned by user */
2369 if (sock_owned_by_user(sk)) {
2370 sk->sk_state = BT_DISCONN;
2371 l2cap_sock_clear_timer(sk);
2372 l2cap_sock_set_timer(sk, HZ / 5);
2373 bh_unlock_sock(sk);
2374 return 0;
2375 }
2376
2377 l2cap_chan_del(sk, ECONNRESET);
2378 bh_unlock_sock(sk);
2379
2380 l2cap_sock_kill(sk);
2381 return 0;
2382 }
2383
2384 static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2385 {
2386 struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
2387 u16 dcid, scid;
2388 struct sock *sk;
2389
2390 scid = __le16_to_cpu(rsp->scid);
2391 dcid = __le16_to_cpu(rsp->dcid);
2392
2393 BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
2394
2395 sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
2396 if (!sk)
2397 return 0;
2398
2399 /* don't delete l2cap channel if sk is owned by user */
2400 if (sock_owned_by_user(sk)) {
2401 sk->sk_state = BT_DISCONN;
2402 l2cap_sock_clear_timer(sk);
2403 l2cap_sock_set_timer(sk, HZ / 5);
2404 bh_unlock_sock(sk);
2405 return 0;
2406 }
2407
2408 l2cap_chan_del(sk, 0);
2409 bh_unlock_sock(sk);
2410
2411 l2cap_sock_kill(sk);
2412 return 0;
2413 }
2414
2415 static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2416 {
2417 struct l2cap_info_req *req = (struct l2cap_info_req *) data;
2418 u16 type;
2419
2420 type = __le16_to_cpu(req->type);
2421
2422 BT_DBG("type 0x%4.4x", type);
2423
2424 if (type == L2CAP_IT_FEAT_MASK) {
2425 u8 buf[8];
2426 u32 feat_mask = l2cap_feat_mask;
2427 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
2428 rsp->type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
2429 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
2430 if (!disable_ertm)
2431 feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
2432 | L2CAP_FEAT_FCS;
2433 put_unaligned_le32(feat_mask, rsp->data);
2434 l2cap_send_cmd(conn, cmd->ident,
2435 L2CAP_INFO_RSP, sizeof(buf), buf);
2436 } else if (type == L2CAP_IT_FIXED_CHAN) {
2437 u8 buf[12];
2438 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
2439 rsp->type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
2440 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
2441 memcpy(buf + 4, l2cap_fixed_chan, 8);
2442 l2cap_send_cmd(conn, cmd->ident,
2443 L2CAP_INFO_RSP, sizeof(buf), buf);
2444 } else {
2445 struct l2cap_info_rsp rsp;
2446 rsp.type = cpu_to_le16(type);
2447 rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP);
2448 l2cap_send_cmd(conn, cmd->ident,
2449 L2CAP_INFO_RSP, sizeof(rsp), &rsp);
2450 }
2451
2452 return 0;
2453 }
2454
2455 static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2456 {
2457 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
2458 u16 type, result;
2459
2460 type = __le16_to_cpu(rsp->type);
2461 result = __le16_to_cpu(rsp->result);
2462
2463 BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
2464
2465 del_timer(&conn->info_timer);
2466
2467 if (result != L2CAP_IR_SUCCESS) {
2468 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2469 conn->info_ident = 0;
2470
2471 l2cap_conn_start(conn);
2472
2473 return 0;
2474 }
2475
2476 if (type == L2CAP_IT_FEAT_MASK) {
2477 conn->feat_mask = get_unaligned_le32(rsp->data);
2478
2479 if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
2480 struct l2cap_info_req req;
2481 req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
2482
2483 conn->info_ident = l2cap_get_ident(conn);
2484
2485 l2cap_send_cmd(conn, conn->info_ident,
2486 L2CAP_INFO_REQ, sizeof(req), &req);
2487 } else {
2488 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2489 conn->info_ident = 0;
2490
2491 l2cap_conn_start(conn);
2492 }
2493 } else if (type == L2CAP_IT_FIXED_CHAN) {
2494 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2495 conn->info_ident = 0;
2496
2497 l2cap_conn_start(conn);
2498 }
2499
2500 return 0;
2501 }
2502
2503 static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency,
2504 u16 to_multiplier)
2505 {
2506 u16 max_latency;
2507
2508 if (min > max || min < 6 || max > 3200)
2509 return -EINVAL;
2510
2511 if (to_multiplier < 10 || to_multiplier > 3200)
2512 return -EINVAL;
2513
2514 if (max >= to_multiplier * 8)
2515 return -EINVAL;
2516
2517 max_latency = (to_multiplier * 8 / max) - 1;
2518 if (latency > 499 || latency > max_latency)
2519 return -EINVAL;
2520
2521 return 0;
2522 }
2523
2524 static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
2525 struct l2cap_cmd_hdr *cmd, u8 *data)
2526 {
2527 struct hci_conn *hcon = conn->hcon;
2528 struct l2cap_conn_param_update_req *req;
2529 struct l2cap_conn_param_update_rsp rsp;
2530 u16 min, max, latency, to_multiplier, cmd_len;
2531 int err;
2532
2533 if (!(hcon->link_mode & HCI_LM_MASTER))
2534 return -EINVAL;
2535
2536 cmd_len = __le16_to_cpu(cmd->len);
2537 if (cmd_len != sizeof(struct l2cap_conn_param_update_req))
2538 return -EPROTO;
2539
2540 req = (struct l2cap_conn_param_update_req *) data;
2541 min = __le16_to_cpu(req->min);
2542 max = __le16_to_cpu(req->max);
2543 latency = __le16_to_cpu(req->latency);
2544 to_multiplier = __le16_to_cpu(req->to_multiplier);
2545
2546 BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x",
2547 min, max, latency, to_multiplier);
2548
2549 memset(&rsp, 0, sizeof(rsp));
2550
2551 err = l2cap_check_conn_param(min, max, latency, to_multiplier);
2552 if (err)
2553 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_REJECTED);
2554 else
2555 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED);
2556
2557 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP,
2558 sizeof(rsp), &rsp);
2559
2560 if (!err)
2561 hci_le_conn_update(hcon, min, max, latency, to_multiplier);
2562
2563 return 0;
2564 }
2565
2566 static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
2567 struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
2568 {
2569 int err = 0;
2570
2571 switch (cmd->code) {
2572 case L2CAP_COMMAND_REJ:
2573 l2cap_command_rej(conn, cmd, data);
2574 break;
2575
2576 case L2CAP_CONN_REQ:
2577 err = l2cap_connect_req(conn, cmd, data);
2578 break;
2579
2580 case L2CAP_CONN_RSP:
2581 err = l2cap_connect_rsp(conn, cmd, data);
2582 break;
2583
2584 case L2CAP_CONF_REQ:
2585 err = l2cap_config_req(conn, cmd, cmd_len, data);
2586 break;
2587
2588 case L2CAP_CONF_RSP:
2589 err = l2cap_config_rsp(conn, cmd, data);
2590 break;
2591
2592 case L2CAP_DISCONN_REQ:
2593 err = l2cap_disconnect_req(conn, cmd, data);
2594 break;
2595
2596 case L2CAP_DISCONN_RSP:
2597 err = l2cap_disconnect_rsp(conn, cmd, data);
2598 break;
2599
2600 case L2CAP_ECHO_REQ:
2601 l2cap_send_cmd(conn, cmd->ident, L2CAP_ECHO_RSP, cmd_len, data);
2602 break;
2603
2604 case L2CAP_ECHO_RSP:
2605 break;
2606
2607 case L2CAP_INFO_REQ:
2608 err = l2cap_information_req(conn, cmd, data);
2609 break;
2610
2611 case L2CAP_INFO_RSP:
2612 err = l2cap_information_rsp(conn, cmd, data);
2613 break;
2614
2615 default:
2616 BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code);
2617 err = -EINVAL;
2618 break;
2619 }
2620
2621 return err;
2622 }
2623
2624 static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn,
2625 struct l2cap_cmd_hdr *cmd, u8 *data)
2626 {
2627 switch (cmd->code) {
2628 case L2CAP_COMMAND_REJ:
2629 return 0;
2630
2631 case L2CAP_CONN_PARAM_UPDATE_REQ:
2632 return l2cap_conn_param_update_req(conn, cmd, data);
2633
2634 case L2CAP_CONN_PARAM_UPDATE_RSP:
2635 return 0;
2636
2637 default:
2638 BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code);
2639 return -EINVAL;
2640 }
2641 }
2642
2643 static inline void l2cap_sig_channel(struct l2cap_conn *conn,
2644 struct sk_buff *skb)
2645 {
2646 u8 *data = skb->data;
2647 int len = skb->len;
2648 struct l2cap_cmd_hdr cmd;
2649 int err;
2650
2651 l2cap_raw_recv(conn, skb);
2652
2653 while (len >= L2CAP_CMD_HDR_SIZE) {
2654 u16 cmd_len;
2655 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
2656 data += L2CAP_CMD_HDR_SIZE;
2657 len -= L2CAP_CMD_HDR_SIZE;
2658
2659 cmd_len = le16_to_cpu(cmd.len);
2660
2661 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len, cmd.ident);
2662
2663 if (cmd_len > len || !cmd.ident) {
2664 BT_DBG("corrupted command");
2665 break;
2666 }
2667
2668 if (conn->hcon->type == LE_LINK)
2669 err = l2cap_le_sig_cmd(conn, &cmd, data);
2670 else
2671 err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data);
2672
2673 if (err) {
2674 struct l2cap_cmd_rej rej;
2675 BT_DBG("error %d", err);
2676
2677 /* FIXME: Map err to a valid reason */
2678 rej.reason = cpu_to_le16(0);
2679 l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
2680 }
2681
2682 data += cmd_len;
2683 len -= cmd_len;
2684 }
2685
2686 kfree_skb(skb);
2687 }
2688
2689 static int l2cap_check_fcs(struct l2cap_pinfo *pi, struct sk_buff *skb)
2690 {
2691 u16 our_fcs, rcv_fcs;
2692 int hdr_size = L2CAP_HDR_SIZE + 2;
2693
2694 if (pi->fcs == L2CAP_FCS_CRC16) {
2695 skb_trim(skb, skb->len - 2);
2696 rcv_fcs = get_unaligned_le16(skb->data + skb->len);
2697 our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
2698
2699 if (our_fcs != rcv_fcs)
2700 return -EBADMSG;
2701 }
2702 return 0;
2703 }
2704
2705 static inline void l2cap_send_i_or_rr_or_rnr(struct sock *sk)
2706 {
2707 struct l2cap_pinfo *pi = l2cap_pi(sk);
2708 u16 control = 0;
2709
2710 pi->frames_sent = 0;
2711
2712 control |= pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
2713
2714 if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
2715 control |= L2CAP_SUPER_RCV_NOT_READY;
2716 l2cap_send_sframe(pi, control);
2717 pi->conn_state |= L2CAP_CONN_RNR_SENT;
2718 }
2719
2720 if (pi->conn_state & L2CAP_CONN_REMOTE_BUSY)
2721 l2cap_retransmit_frames(sk);
2722
2723 l2cap_ertm_send(sk);
2724
2725 if (!(pi->conn_state & L2CAP_CONN_LOCAL_BUSY) &&
2726 pi->frames_sent == 0) {
2727 control |= L2CAP_SUPER_RCV_READY;
2728 l2cap_send_sframe(pi, control);
2729 }
2730 }
2731
2732 static int l2cap_add_to_srej_queue(struct sock *sk, struct sk_buff *skb, u8 tx_seq, u8 sar)
2733 {
2734 struct sk_buff *next_skb;
2735 struct l2cap_pinfo *pi = l2cap_pi(sk);
2736 int tx_seq_offset, next_tx_seq_offset;
2737
2738 bt_cb(skb)->tx_seq = tx_seq;
2739 bt_cb(skb)->sar = sar;
2740
2741 next_skb = skb_peek(SREJ_QUEUE(sk));
2742 if (!next_skb) {
2743 __skb_queue_tail(SREJ_QUEUE(sk), skb);
2744 return 0;
2745 }
2746
2747 tx_seq_offset = (tx_seq - pi->buffer_seq) % 64;
2748 if (tx_seq_offset < 0)
2749 tx_seq_offset += 64;
2750
2751 do {
2752 if (bt_cb(next_skb)->tx_seq == tx_seq)
2753 return -EINVAL;
2754
2755 next_tx_seq_offset = (bt_cb(next_skb)->tx_seq -
2756 pi->buffer_seq) % 64;
2757 if (next_tx_seq_offset < 0)
2758 next_tx_seq_offset += 64;
2759
2760 if (next_tx_seq_offset > tx_seq_offset) {
2761 __skb_queue_before(SREJ_QUEUE(sk), next_skb, skb);
2762 return 0;
2763 }
2764
2765 if (skb_queue_is_last(SREJ_QUEUE(sk), next_skb))
2766 break;
2767
2768 } while ((next_skb = skb_queue_next(SREJ_QUEUE(sk), next_skb)));
2769
2770 __skb_queue_tail(SREJ_QUEUE(sk), skb);
2771
2772 return 0;
2773 }
2774
2775 static int l2cap_ertm_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control)
2776 {
2777 struct l2cap_pinfo *pi = l2cap_pi(sk);
2778 struct sk_buff *_skb;
2779 int err;
2780
2781 switch (control & L2CAP_CTRL_SAR) {
2782 case L2CAP_SDU_UNSEGMENTED:
2783 if (pi->conn_state & L2CAP_CONN_SAR_SDU)
2784 goto drop;
2785
2786 err = sock_queue_rcv_skb(sk, skb);
2787 if (!err)
2788 return err;
2789
2790 break;
2791
2792 case L2CAP_SDU_START:
2793 if (pi->conn_state & L2CAP_CONN_SAR_SDU)
2794 goto drop;
2795
2796 pi->sdu_len = get_unaligned_le16(skb->data);
2797
2798 if (pi->sdu_len > pi->imtu)
2799 goto disconnect;
2800
2801 pi->sdu = bt_skb_alloc(pi->sdu_len, GFP_ATOMIC);
2802 if (!pi->sdu)
2803 return -ENOMEM;
2804
2805 /* pull sdu_len bytes only after alloc, because of Local Busy
2806 * condition we have to be sure that this will be executed
2807 * only once, i.e., when alloc does not fail */
2808 skb_pull(skb, 2);
2809
2810 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
2811
2812 pi->conn_state |= L2CAP_CONN_SAR_SDU;
2813 pi->partial_sdu_len = skb->len;
2814 break;
2815
2816 case L2CAP_SDU_CONTINUE:
2817 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
2818 goto disconnect;
2819
2820 if (!pi->sdu)
2821 goto disconnect;
2822
2823 pi->partial_sdu_len += skb->len;
2824 if (pi->partial_sdu_len > pi->sdu_len)
2825 goto drop;
2826
2827 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
2828
2829 break;
2830
2831 case L2CAP_SDU_END:
2832 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
2833 goto disconnect;
2834
2835 if (!pi->sdu)
2836 goto disconnect;
2837
2838 if (!(pi->conn_state & L2CAP_CONN_SAR_RETRY)) {
2839 pi->partial_sdu_len += skb->len;
2840
2841 if (pi->partial_sdu_len > pi->imtu)
2842 goto drop;
2843
2844 if (pi->partial_sdu_len != pi->sdu_len)
2845 goto drop;
2846
2847 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
2848 }
2849
2850 _skb = skb_clone(pi->sdu, GFP_ATOMIC);
2851 if (!_skb) {
2852 pi->conn_state |= L2CAP_CONN_SAR_RETRY;
2853 return -ENOMEM;
2854 }
2855
2856 err = sock_queue_rcv_skb(sk, _skb);
2857 if (err < 0) {
2858 kfree_skb(_skb);
2859 pi->conn_state |= L2CAP_CONN_SAR_RETRY;
2860 return err;
2861 }
2862
2863 pi->conn_state &= ~L2CAP_CONN_SAR_RETRY;
2864 pi->conn_state &= ~L2CAP_CONN_SAR_SDU;
2865
2866 kfree_skb(pi->sdu);
2867 break;
2868 }
2869
2870 kfree_skb(skb);
2871 return 0;
2872
2873 drop:
2874 kfree_skb(pi->sdu);
2875 pi->sdu = NULL;
2876
2877 disconnect:
2878 l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
2879 kfree_skb(skb);
2880 return 0;
2881 }
2882
2883 static int l2cap_try_push_rx_skb(struct sock *sk)
2884 {
2885 struct l2cap_pinfo *pi = l2cap_pi(sk);
2886 struct sk_buff *skb;
2887 u16 control;
2888 int err;
2889
2890 while ((skb = skb_dequeue(BUSY_QUEUE(sk)))) {
2891 control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
2892 err = l2cap_ertm_reassembly_sdu(sk, skb, control);
2893 if (err < 0) {
2894 skb_queue_head(BUSY_QUEUE(sk), skb);
2895 return -EBUSY;
2896 }
2897
2898 pi->buffer_seq = (pi->buffer_seq + 1) % 64;
2899 }
2900
2901 if (!(pi->conn_state & L2CAP_CONN_RNR_SENT))
2902 goto done;
2903
2904 control = pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
2905 control |= L2CAP_SUPER_RCV_READY | L2CAP_CTRL_POLL;
2906 l2cap_send_sframe(pi, control);
2907 l2cap_pi(sk)->retry_count = 1;
2908
2909 del_timer(&pi->retrans_timer);
2910 __mod_monitor_timer();
2911
2912 l2cap_pi(sk)->conn_state |= L2CAP_CONN_WAIT_F;
2913
2914 done:
2915 pi->conn_state &= ~L2CAP_CONN_LOCAL_BUSY;
2916 pi->conn_state &= ~L2CAP_CONN_RNR_SENT;
2917
2918 BT_DBG("sk %p, Exit local busy", sk);
2919
2920 return 0;
2921 }
2922
2923 static void l2cap_busy_work(struct work_struct *work)
2924 {
2925 DECLARE_WAITQUEUE(wait, current);
2926 struct l2cap_pinfo *pi =
2927 container_of(work, struct l2cap_pinfo, busy_work);
2928 struct sock *sk = (struct sock *)pi;
2929 int n_tries = 0, timeo = HZ/5, err;
2930 struct sk_buff *skb;
2931
2932 lock_sock(sk);
2933
2934 add_wait_queue(sk_sleep(sk), &wait);
2935 while ((skb = skb_peek(BUSY_QUEUE(sk)))) {
2936 set_current_state(TASK_INTERRUPTIBLE);
2937
2938 if (n_tries++ > L2CAP_LOCAL_BUSY_TRIES) {
2939 err = -EBUSY;
2940 l2cap_send_disconn_req(pi->conn, sk, EBUSY);
2941 break;
2942 }
2943
2944 if (!timeo)
2945 timeo = HZ/5;
2946
2947 if (signal_pending(current)) {
2948 err = sock_intr_errno(timeo);
2949 break;
2950 }
2951
2952 release_sock(sk);
2953 timeo = schedule_timeout(timeo);
2954 lock_sock(sk);
2955
2956 err = sock_error(sk);
2957 if (err)
2958 break;
2959
2960 if (l2cap_try_push_rx_skb(sk) == 0)
2961 break;
2962 }
2963
2964 set_current_state(TASK_RUNNING);
2965 remove_wait_queue(sk_sleep(sk), &wait);
2966
2967 release_sock(sk);
2968 }
2969
2970 static int l2cap_push_rx_skb(struct sock *sk, struct sk_buff *skb, u16 control)
2971 {
2972 struct l2cap_pinfo *pi = l2cap_pi(sk);
2973 int sctrl, err;
2974
2975 if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
2976 bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT;
2977 __skb_queue_tail(BUSY_QUEUE(sk), skb);
2978 return l2cap_try_push_rx_skb(sk);
2979
2980
2981 }
2982
2983 err = l2cap_ertm_reassembly_sdu(sk, skb, control);
2984 if (err >= 0) {
2985 pi->buffer_seq = (pi->buffer_seq + 1) % 64;
2986 return err;
2987 }
2988
2989 /* Busy Condition */
2990 BT_DBG("sk %p, Enter local busy", sk);
2991
2992 pi->conn_state |= L2CAP_CONN_LOCAL_BUSY;
2993 bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT;
2994 __skb_queue_tail(BUSY_QUEUE(sk), skb);
2995
2996 sctrl = pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
2997 sctrl |= L2CAP_SUPER_RCV_NOT_READY;
2998 l2cap_send_sframe(pi, sctrl);
2999
3000 pi->conn_state |= L2CAP_CONN_RNR_SENT;
3001
3002 del_timer(&pi->ack_timer);
3003
3004 queue_work(_busy_wq, &pi->busy_work);
3005
3006 return err;
3007 }
3008
3009 static int l2cap_streaming_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control)
3010 {
3011 struct l2cap_pinfo *pi = l2cap_pi(sk);
3012 struct sk_buff *_skb;
3013 int err = -EINVAL;
3014
3015 /*
3016 * TODO: We have to notify the userland if some data is lost with the
3017 * Streaming Mode.
3018 */
3019
3020 switch (control & L2CAP_CTRL_SAR) {
3021 case L2CAP_SDU_UNSEGMENTED:
3022 if (pi->conn_state & L2CAP_CONN_SAR_SDU) {
3023 kfree_skb(pi->sdu);
3024 break;
3025 }
3026
3027 err = sock_queue_rcv_skb(sk, skb);
3028 if (!err)
3029 return 0;
3030
3031 break;
3032
3033 case L2CAP_SDU_START:
3034 if (pi->conn_state & L2CAP_CONN_SAR_SDU) {
3035 kfree_skb(pi->sdu);
3036 break;
3037 }
3038
3039 pi->sdu_len = get_unaligned_le16(skb->data);
3040 skb_pull(skb, 2);
3041
3042 if (pi->sdu_len > pi->imtu) {
3043 err = -EMSGSIZE;
3044 break;
3045 }
3046
3047 pi->sdu = bt_skb_alloc(pi->sdu_len, GFP_ATOMIC);
3048 if (!pi->sdu) {
3049 err = -ENOMEM;
3050 break;
3051 }
3052
3053 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3054
3055 pi->conn_state |= L2CAP_CONN_SAR_SDU;
3056 pi->partial_sdu_len = skb->len;
3057 err = 0;
3058 break;
3059
3060 case L2CAP_SDU_CONTINUE:
3061 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
3062 break;
3063
3064 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3065
3066 pi->partial_sdu_len += skb->len;
3067 if (pi->partial_sdu_len > pi->sdu_len)
3068 kfree_skb(pi->sdu);
3069 else
3070 err = 0;
3071
3072 break;
3073
3074 case L2CAP_SDU_END:
3075 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
3076 break;
3077
3078 memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
3079
3080 pi->conn_state &= ~L2CAP_CONN_SAR_SDU;
3081 pi->partial_sdu_len += skb->len;
3082
3083 if (pi->partial_sdu_len > pi->imtu)
3084 goto drop;
3085
3086 if (pi->partial_sdu_len == pi->sdu_len) {
3087 _skb = skb_clone(pi->sdu, GFP_ATOMIC);
3088 err = sock_queue_rcv_skb(sk, _skb);
3089 if (err < 0)
3090 kfree_skb(_skb);
3091 }
3092 err = 0;
3093
3094 drop:
3095 kfree_skb(pi->sdu);
3096 break;
3097 }
3098
3099 kfree_skb(skb);
3100 return err;
3101 }
3102
3103 static void l2cap_check_srej_gap(struct sock *sk, u8 tx_seq)
3104 {
3105 struct sk_buff *skb;
3106 u16 control;
3107
3108 while ((skb = skb_peek(SREJ_QUEUE(sk)))) {
3109 if (bt_cb(skb)->tx_seq != tx_seq)
3110 break;
3111
3112 skb = skb_dequeue(SREJ_QUEUE(sk));
3113 control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
3114 l2cap_ertm_reassembly_sdu(sk, skb, control);
3115 l2cap_pi(sk)->buffer_seq_srej =
3116 (l2cap_pi(sk)->buffer_seq_srej + 1) % 64;
3117 tx_seq = (tx_seq + 1) % 64;
3118 }
3119 }
3120
3121 static void l2cap_resend_srejframe(struct sock *sk, u8 tx_seq)
3122 {
3123 struct l2cap_pinfo *pi = l2cap_pi(sk);
3124 struct srej_list *l, *tmp;
3125 u16 control;
3126
3127 list_for_each_entry_safe(l, tmp, SREJ_LIST(sk), list) {
3128 if (l->tx_seq == tx_seq) {
3129 list_del(&l->list);
3130 kfree(l);
3131 return;
3132 }
3133 control = L2CAP_SUPER_SELECT_REJECT;
3134 control |= l->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3135 l2cap_send_sframe(pi, control);
3136 list_del(&l->list);
3137 list_add_tail(&l->list, SREJ_LIST(sk));
3138 }
3139 }
3140
3141 static void l2cap_send_srejframe(struct sock *sk, u8 tx_seq)
3142 {
3143 struct l2cap_pinfo *pi = l2cap_pi(sk);
3144 struct srej_list *new;
3145 u16 control;
3146
3147 while (tx_seq != pi->expected_tx_seq) {
3148 control = L2CAP_SUPER_SELECT_REJECT;
3149 control |= pi->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
3150 l2cap_send_sframe(pi, control);
3151
3152 new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC);
3153 new->tx_seq = pi->expected_tx_seq;
3154 pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
3155 list_add_tail(&new->list, SREJ_LIST(sk));
3156 }
3157 pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
3158 }
3159
3160 static inline int l2cap_data_channel_iframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
3161 {
3162 struct l2cap_pinfo *pi = l2cap_pi(sk);
3163 u8 tx_seq = __get_txseq(rx_control);
3164 u8 req_seq = __get_reqseq(rx_control);
3165 u8 sar = rx_control >> L2CAP_CTRL_SAR_SHIFT;
3166 int tx_seq_offset, expected_tx_seq_offset;
3167 int num_to_ack = (pi->tx_win/6) + 1;
3168 int err = 0;
3169
3170 BT_DBG("sk %p len %d tx_seq %d rx_control 0x%4.4x", sk, skb->len, tx_seq,
3171 rx_control);
3172
3173 if (L2CAP_CTRL_FINAL & rx_control &&
3174 l2cap_pi(sk)->conn_state & L2CAP_CONN_WAIT_F) {
3175 del_timer(&pi->monitor_timer);
3176 if (pi->unacked_frames > 0)
3177 __mod_retrans_timer();
3178 pi->conn_state &= ~L2CAP_CONN_WAIT_F;
3179 }
3180
3181 pi->expected_ack_seq = req_seq;
3182 l2cap_drop_acked_frames(sk);
3183
3184 if (tx_seq == pi->expected_tx_seq)
3185 goto expected;
3186
3187 tx_seq_offset = (tx_seq - pi->buffer_seq) % 64;
3188 if (tx_seq_offset < 0)
3189 tx_seq_offset += 64;
3190
3191 /* invalid tx_seq */
3192 if (tx_seq_offset >= pi->tx_win) {
3193 l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
3194 goto drop;
3195 }
3196
3197 if (pi->conn_state == L2CAP_CONN_LOCAL_BUSY)
3198 goto drop;
3199
3200 if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
3201 struct srej_list *first;
3202
3203 first = list_first_entry(SREJ_LIST(sk),
3204 struct srej_list, list);
3205 if (tx_seq == first->tx_seq) {
3206 l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
3207 l2cap_check_srej_gap(sk, tx_seq);
3208
3209 list_del(&first->list);
3210 kfree(first);
3211
3212 if (list_empty(SREJ_LIST(sk))) {
3213 pi->buffer_seq = pi->buffer_seq_srej;
3214 pi->conn_state &= ~L2CAP_CONN_SREJ_SENT;
3215 l2cap_send_ack(pi);
3216 BT_DBG("sk %p, Exit SREJ_SENT", sk);
3217 }
3218 } else {
3219 struct srej_list *l;
3220
3221 /* duplicated tx_seq */
3222 if (l2cap_add_to_srej_queue(sk, skb, tx_seq, sar) < 0)
3223 goto drop;
3224
3225 list_for_each_entry(l, SREJ_LIST(sk), list) {
3226 if (l->tx_seq == tx_seq) {
3227 l2cap_resend_srejframe(sk, tx_seq);
3228 return 0;
3229 }
3230 }
3231 l2cap_send_srejframe(sk, tx_seq);
3232 }
3233 } else {
3234 expected_tx_seq_offset =
3235 (pi->expected_tx_seq - pi->buffer_seq) % 64;
3236 if (expected_tx_seq_offset < 0)
3237 expected_tx_seq_offset += 64;
3238
3239 /* duplicated tx_seq */
3240 if (tx_seq_offset < expected_tx_seq_offset)
3241 goto drop;
3242
3243 pi->conn_state |= L2CAP_CONN_SREJ_SENT;
3244
3245 BT_DBG("sk %p, Enter SREJ", sk);
3246
3247 INIT_LIST_HEAD(SREJ_LIST(sk));
3248 pi->buffer_seq_srej = pi->buffer_seq;
3249
3250 __skb_queue_head_init(SREJ_QUEUE(sk));
3251 __skb_queue_head_init(BUSY_QUEUE(sk));
3252 l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
3253
3254 pi->conn_state |= L2CAP_CONN_SEND_PBIT;
3255
3256 l2cap_send_srejframe(sk, tx_seq);
3257
3258 del_timer(&pi->ack_timer);
3259 }
3260 return 0;
3261
3262 expected:
3263 pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
3264
3265 if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
3266 bt_cb(skb)->tx_seq = tx_seq;
3267 bt_cb(skb)->sar = sar;
3268 __skb_queue_tail(SREJ_QUEUE(sk), skb);
3269 return 0;
3270 }
3271
3272 err = l2cap_push_rx_skb(sk, skb, rx_control);
3273 if (err < 0)
3274 return 0;
3275
3276 if (rx_control & L2CAP_CTRL_FINAL) {
3277 if (pi->conn_state & L2CAP_CONN_REJ_ACT)
3278 pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
3279 else
3280 l2cap_retransmit_frames(sk);
3281 }
3282
3283 __mod_ack_timer();
3284
3285 pi->num_acked = (pi->num_acked + 1) % num_to_ack;
3286 if (pi->num_acked == num_to_ack - 1)
3287 l2cap_send_ack(pi);
3288
3289 return 0;
3290
3291 drop:
3292 kfree_skb(skb);
3293 return 0;
3294 }
3295
3296 static inline void l2cap_data_channel_rrframe(struct sock *sk, u16 rx_control)
3297 {
3298 struct l2cap_pinfo *pi = l2cap_pi(sk);
3299
3300 BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, __get_reqseq(rx_control),
3301 rx_control);
3302
3303 pi->expected_ack_seq = __get_reqseq(rx_control);
3304 l2cap_drop_acked_frames(sk);
3305
3306 if (rx_control & L2CAP_CTRL_POLL) {
3307 pi->conn_state |= L2CAP_CONN_SEND_FBIT;
3308 if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
3309 if ((pi->conn_state & L2CAP_CONN_REMOTE_BUSY) &&
3310 (pi->unacked_frames > 0))
3311 __mod_retrans_timer();
3312
3313 pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3314 l2cap_send_srejtail(sk);
3315 } else {
3316 l2cap_send_i_or_rr_or_rnr(sk);
3317 }
3318
3319 } else if (rx_control & L2CAP_CTRL_FINAL) {
3320 pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3321
3322 if (pi->conn_state & L2CAP_CONN_REJ_ACT)
3323 pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
3324 else
3325 l2cap_retransmit_frames(sk);
3326
3327 } else {
3328 if ((pi->conn_state & L2CAP_CONN_REMOTE_BUSY) &&
3329 (pi->unacked_frames > 0))
3330 __mod_retrans_timer();
3331
3332 pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3333 if (pi->conn_state & L2CAP_CONN_SREJ_SENT)
3334 l2cap_send_ack(pi);
3335 else
3336 l2cap_ertm_send(sk);
3337 }
3338 }
3339
3340 static inline void l2cap_data_channel_rejframe(struct sock *sk, u16 rx_control)
3341 {
3342 struct l2cap_pinfo *pi = l2cap_pi(sk);
3343 u8 tx_seq = __get_reqseq(rx_control);
3344
3345 BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, tx_seq, rx_control);
3346
3347 pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3348
3349 pi->expected_ack_seq = tx_seq;
3350 l2cap_drop_acked_frames(sk);
3351
3352 if (rx_control & L2CAP_CTRL_FINAL) {
3353 if (pi->conn_state & L2CAP_CONN_REJ_ACT)
3354 pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
3355 else
3356 l2cap_retransmit_frames(sk);
3357 } else {
3358 l2cap_retransmit_frames(sk);
3359
3360 if (pi->conn_state & L2CAP_CONN_WAIT_F)
3361 pi->conn_state |= L2CAP_CONN_REJ_ACT;
3362 }
3363 }
3364 static inline void l2cap_data_channel_srejframe(struct sock *sk, u16 rx_control)
3365 {
3366 struct l2cap_pinfo *pi = l2cap_pi(sk);
3367 u8 tx_seq = __get_reqseq(rx_control);
3368
3369 BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, tx_seq, rx_control);
3370
3371 pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3372
3373 if (rx_control & L2CAP_CTRL_POLL) {
3374 pi->expected_ack_seq = tx_seq;
3375 l2cap_drop_acked_frames(sk);
3376
3377 pi->conn_state |= L2CAP_CONN_SEND_FBIT;
3378 l2cap_retransmit_one_frame(sk, tx_seq);
3379
3380 l2cap_ertm_send(sk);
3381
3382 if (pi->conn_state & L2CAP_CONN_WAIT_F) {
3383 pi->srej_save_reqseq = tx_seq;
3384 pi->conn_state |= L2CAP_CONN_SREJ_ACT;
3385 }
3386 } else if (rx_control & L2CAP_CTRL_FINAL) {
3387 if ((pi->conn_state & L2CAP_CONN_SREJ_ACT) &&
3388 pi->srej_save_reqseq == tx_seq)
3389 pi->conn_state &= ~L2CAP_CONN_SREJ_ACT;
3390 else
3391 l2cap_retransmit_one_frame(sk, tx_seq);
3392 } else {
3393 l2cap_retransmit_one_frame(sk, tx_seq);
3394 if (pi->conn_state & L2CAP_CONN_WAIT_F) {
3395 pi->srej_save_reqseq = tx_seq;
3396 pi->conn_state |= L2CAP_CONN_SREJ_ACT;
3397 }
3398 }
3399 }
3400
3401 static inline void l2cap_data_channel_rnrframe(struct sock *sk, u16 rx_control)
3402 {
3403 struct l2cap_pinfo *pi = l2cap_pi(sk);
3404 u8 tx_seq = __get_reqseq(rx_control);
3405
3406 BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, tx_seq, rx_control);
3407
3408 pi->conn_state |= L2CAP_CONN_REMOTE_BUSY;
3409 pi->expected_ack_seq = tx_seq;
3410 l2cap_drop_acked_frames(sk);
3411
3412 if (rx_control & L2CAP_CTRL_POLL)
3413 pi->conn_state |= L2CAP_CONN_SEND_FBIT;
3414
3415 if (!(pi->conn_state & L2CAP_CONN_SREJ_SENT)) {
3416 del_timer(&pi->retrans_timer);
3417 if (rx_control & L2CAP_CTRL_POLL)
3418 l2cap_send_rr_or_rnr(pi, L2CAP_CTRL_FINAL);
3419 return;
3420 }
3421
3422 if (rx_control & L2CAP_CTRL_POLL)
3423 l2cap_send_srejtail(sk);
3424 else
3425 l2cap_send_sframe(pi, L2CAP_SUPER_RCV_READY);
3426 }
3427
3428 static inline int l2cap_data_channel_sframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
3429 {
3430 BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
3431
3432 if (L2CAP_CTRL_FINAL & rx_control &&
3433 l2cap_pi(sk)->conn_state & L2CAP_CONN_WAIT_F) {
3434 del_timer(&l2cap_pi(sk)->monitor_timer);
3435 if (l2cap_pi(sk)->unacked_frames > 0)
3436 __mod_retrans_timer();
3437 l2cap_pi(sk)->conn_state &= ~L2CAP_CONN_WAIT_F;
3438 }
3439
3440 switch (rx_control & L2CAP_CTRL_SUPERVISE) {
3441 case L2CAP_SUPER_RCV_READY:
3442 l2cap_data_channel_rrframe(sk, rx_control);
3443 break;
3444
3445 case L2CAP_SUPER_REJECT:
3446 l2cap_data_channel_rejframe(sk, rx_control);
3447 break;
3448
3449 case L2CAP_SUPER_SELECT_REJECT:
3450 l2cap_data_channel_srejframe(sk, rx_control);
3451 break;
3452
3453 case L2CAP_SUPER_RCV_NOT_READY:
3454 l2cap_data_channel_rnrframe(sk, rx_control);
3455 break;
3456 }
3457
3458 kfree_skb(skb);
3459 return 0;
3460 }
3461
3462 static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb)
3463 {
3464 struct l2cap_pinfo *pi = l2cap_pi(sk);
3465 u16 control;
3466 u8 req_seq;
3467 int len, next_tx_seq_offset, req_seq_offset;
3468
3469 control = get_unaligned_le16(skb->data);
3470 skb_pull(skb, 2);
3471 len = skb->len;
3472
3473 /*
3474 * We can just drop the corrupted I-frame here.
3475 * Receiver will miss it and start proper recovery
3476 * procedures and ask retransmission.
3477 */
3478 if (l2cap_check_fcs(pi, skb))
3479 goto drop;
3480
3481 if (__is_sar_start(control) && __is_iframe(control))
3482 len -= 2;
3483
3484 if (pi->fcs == L2CAP_FCS_CRC16)
3485 len -= 2;
3486
3487 if (len > pi->mps) {
3488 l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
3489 goto drop;
3490 }
3491
3492 req_seq = __get_reqseq(control);
3493 req_seq_offset = (req_seq - pi->expected_ack_seq) % 64;
3494 if (req_seq_offset < 0)
3495 req_seq_offset += 64;
3496
3497 next_tx_seq_offset =
3498 (pi->next_tx_seq - pi->expected_ack_seq) % 64;
3499 if (next_tx_seq_offset < 0)
3500 next_tx_seq_offset += 64;
3501
3502 /* check for invalid req-seq */
3503 if (req_seq_offset > next_tx_seq_offset) {
3504 l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
3505 goto drop;
3506 }
3507
3508 if (__is_iframe(control)) {
3509 if (len < 0) {
3510 l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
3511 goto drop;
3512 }
3513
3514 l2cap_data_channel_iframe(sk, control, skb);
3515 } else {
3516 if (len != 0) {
3517 BT_ERR("%d", len);
3518 l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
3519 goto drop;
3520 }
3521
3522 l2cap_data_channel_sframe(sk, control, skb);
3523 }
3524
3525 return 0;
3526
3527 drop:
3528 kfree_skb(skb);
3529 return 0;
3530 }
3531
3532 static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
3533 {
3534 struct sock *sk;
3535 struct l2cap_pinfo *pi;
3536 u16 control;
3537 u8 tx_seq;
3538 int len;
3539
3540 sk = l2cap_get_chan_by_scid(&conn->chan_list, cid);
3541 if (!sk) {
3542 BT_DBG("unknown cid 0x%4.4x", cid);
3543 goto drop;
3544 }
3545
3546 pi = l2cap_pi(sk);
3547
3548 BT_DBG("sk %p, len %d", sk, skb->len);
3549
3550 if (sk->sk_state != BT_CONNECTED)
3551 goto drop;
3552
3553 switch (pi->mode) {
3554 case L2CAP_MODE_BASIC:
3555 /* If socket recv buffers overflows we drop data here
3556 * which is *bad* because L2CAP has to be reliable.
3557 * But we don't have any other choice. L2CAP doesn't
3558 * provide flow control mechanism. */
3559
3560 if (pi->imtu < skb->len)
3561 goto drop;
3562
3563 if (!sock_queue_rcv_skb(sk, skb))
3564 goto done;
3565 break;
3566
3567 case L2CAP_MODE_ERTM:
3568 if (!sock_owned_by_user(sk)) {
3569 l2cap_ertm_data_rcv(sk, skb);
3570 } else {
3571 if (sk_add_backlog(sk, skb))
3572 goto drop;
3573 }
3574
3575 goto done;
3576
3577 case L2CAP_MODE_STREAMING:
3578 control = get_unaligned_le16(skb->data);
3579 skb_pull(skb, 2);
3580 len = skb->len;
3581
3582 if (l2cap_check_fcs(pi, skb))
3583 goto drop;
3584
3585 if (__is_sar_start(control))
3586 len -= 2;
3587
3588 if (pi->fcs == L2CAP_FCS_CRC16)
3589 len -= 2;
3590
3591 if (len > pi->mps || len < 0 || __is_sframe(control))
3592 goto drop;
3593
3594 tx_seq = __get_txseq(control);
3595
3596 if (pi->expected_tx_seq == tx_seq)
3597 pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
3598 else
3599 pi->expected_tx_seq = (tx_seq + 1) % 64;
3600
3601 l2cap_streaming_reassembly_sdu(sk, skb, control);
3602
3603 goto done;
3604
3605 default:
3606 BT_DBG("sk %p: bad mode 0x%2.2x", sk, pi->mode);
3607 break;
3608 }
3609
3610 drop:
3611 kfree_skb(skb);
3612
3613 done:
3614 if (sk)
3615 bh_unlock_sock(sk);
3616
3617 return 0;
3618 }
3619
3620 static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb)
3621 {
3622 struct sock *sk;
3623
3624 sk = l2cap_get_sock_by_psm(0, psm, conn->src);
3625 if (!sk)
3626 goto drop;
3627
3628 bh_lock_sock(sk);
3629
3630 BT_DBG("sk %p, len %d", sk, skb->len);
3631
3632 if (sk->sk_state != BT_BOUND && sk->sk_state != BT_CONNECTED)
3633 goto drop;
3634
3635 if (l2cap_pi(sk)->imtu < skb->len)
3636 goto drop;
3637
3638 if (!sock_queue_rcv_skb(sk, skb))
3639 goto done;
3640
3641 drop:
3642 kfree_skb(skb);
3643
3644 done:
3645 if (sk)
3646 bh_unlock_sock(sk);
3647 return 0;
3648 }
3649
3650 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
3651 {
3652 struct l2cap_hdr *lh = (void *) skb->data;
3653 u16 cid, len;
3654 __le16 psm;
3655
3656 skb_pull(skb, L2CAP_HDR_SIZE);
3657 cid = __le16_to_cpu(lh->cid);
3658 len = __le16_to_cpu(lh->len);
3659
3660 if (len != skb->len) {
3661 kfree_skb(skb);
3662 return;
3663 }
3664
3665 BT_DBG("len %d, cid 0x%4.4x", len, cid);
3666
3667 switch (cid) {
3668 case L2CAP_CID_LE_SIGNALING:
3669 case L2CAP_CID_SIGNALING:
3670 l2cap_sig_channel(conn, skb);
3671 break;
3672
3673 case L2CAP_CID_CONN_LESS:
3674 psm = get_unaligned_le16(skb->data);
3675 skb_pull(skb, 2);
3676 l2cap_conless_channel(conn, psm, skb);
3677 break;
3678
3679 default:
3680 l2cap_data_channel(conn, cid, skb);
3681 break;
3682 }
3683 }
3684
3685 /* ---- L2CAP interface with lower layer (HCI) ---- */
3686
3687 static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
3688 {
3689 int exact = 0, lm1 = 0, lm2 = 0;
3690 register struct sock *sk;
3691 struct hlist_node *node;
3692
3693 if (type != ACL_LINK)
3694 return -EINVAL;
3695
3696 BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
3697
3698 /* Find listening sockets and check their link_mode */
3699 read_lock(&l2cap_sk_list.lock);
3700 sk_for_each(sk, node, &l2cap_sk_list.head) {
3701 if (sk->sk_state != BT_LISTEN)
3702 continue;
3703
3704 if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
3705 lm1 |= HCI_LM_ACCEPT;
3706 if (l2cap_pi(sk)->role_switch)
3707 lm1 |= HCI_LM_MASTER;
3708 exact++;
3709 } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) {
3710 lm2 |= HCI_LM_ACCEPT;
3711 if (l2cap_pi(sk)->role_switch)
3712 lm2 |= HCI_LM_MASTER;
3713 }
3714 }
3715 read_unlock(&l2cap_sk_list.lock);
3716
3717 return exact ? lm1 : lm2;
3718 }
3719
3720 static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
3721 {
3722 struct l2cap_conn *conn;
3723
3724 BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
3725
3726 if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK))
3727 return -EINVAL;
3728
3729 if (!status) {
3730 conn = l2cap_conn_add(hcon, status);
3731 if (conn)
3732 l2cap_conn_ready(conn);
3733 } else
3734 l2cap_conn_del(hcon, bt_err(status));
3735
3736 return 0;
3737 }
3738
3739 static int l2cap_disconn_ind(struct hci_conn *hcon)
3740 {
3741 struct l2cap_conn *conn = hcon->l2cap_data;
3742
3743 BT_DBG("hcon %p", hcon);
3744
3745 if (hcon->type != ACL_LINK || !conn)
3746 return 0x13;
3747
3748 return conn->disc_reason;
3749 }
3750
3751 static int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
3752 {
3753 BT_DBG("hcon %p reason %d", hcon, reason);
3754
3755 if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK))
3756 return -EINVAL;
3757
3758 l2cap_conn_del(hcon, bt_err(reason));
3759
3760 return 0;
3761 }
3762
3763 static inline void l2cap_check_encryption(struct sock *sk, u8 encrypt)
3764 {
3765 if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM)
3766 return;
3767
3768 if (encrypt == 0x00) {
3769 if (l2cap_pi(sk)->sec_level == BT_SECURITY_MEDIUM) {
3770 l2cap_sock_clear_timer(sk);
3771 l2cap_sock_set_timer(sk, HZ * 5);
3772 } else if (l2cap_pi(sk)->sec_level == BT_SECURITY_HIGH)
3773 __l2cap_sock_close(sk, ECONNREFUSED);
3774 } else {
3775 if (l2cap_pi(sk)->sec_level == BT_SECURITY_MEDIUM)
3776 l2cap_sock_clear_timer(sk);
3777 }
3778 }
3779
3780 static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
3781 {
3782 struct l2cap_chan_list *l;
3783 struct l2cap_conn *conn = hcon->l2cap_data;
3784 struct sock *sk;
3785
3786 if (!conn)
3787 return 0;
3788
3789 l = &conn->chan_list;
3790
3791 BT_DBG("conn %p", conn);
3792
3793 read_lock(&l->lock);
3794
3795 for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
3796 bh_lock_sock(sk);
3797
3798 if (l2cap_pi(sk)->conf_state & L2CAP_CONF_CONNECT_PEND) {
3799 bh_unlock_sock(sk);
3800 continue;
3801 }
3802
3803 if (!status && (sk->sk_state == BT_CONNECTED ||
3804 sk->sk_state == BT_CONFIG)) {
3805 l2cap_check_encryption(sk, encrypt);
3806 bh_unlock_sock(sk);
3807 continue;
3808 }
3809
3810 if (sk->sk_state == BT_CONNECT) {
3811 if (!status) {
3812 struct l2cap_conn_req req;
3813 req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
3814 req.psm = l2cap_pi(sk)->psm;
3815
3816 l2cap_pi(sk)->ident = l2cap_get_ident(conn);
3817 l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND;
3818
3819 l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
3820 L2CAP_CONN_REQ, sizeof(req), &req);
3821 } else {
3822 l2cap_sock_clear_timer(sk);
3823 l2cap_sock_set_timer(sk, HZ / 10);
3824 }
3825 } else if (sk->sk_state == BT_CONNECT2) {
3826 struct l2cap_conn_rsp rsp;
3827 __u16 result;
3828
3829 if (!status) {
3830 sk->sk_state = BT_CONFIG;
3831 result = L2CAP_CR_SUCCESS;
3832 } else {
3833 sk->sk_state = BT_DISCONN;
3834 l2cap_sock_set_timer(sk, HZ / 10);
3835 result = L2CAP_CR_SEC_BLOCK;
3836 }
3837
3838 rsp.scid = cpu_to_le16(l2cap_pi(sk)->dcid);
3839 rsp.dcid = cpu_to_le16(l2cap_pi(sk)->scid);
3840 rsp.result = cpu_to_le16(result);
3841 rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
3842 l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
3843 L2CAP_CONN_RSP, sizeof(rsp), &rsp);
3844 }
3845
3846 bh_unlock_sock(sk);
3847 }
3848
3849 read_unlock(&l->lock);
3850
3851 return 0;
3852 }
3853
3854 static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
3855 {
3856 struct l2cap_conn *conn = hcon->l2cap_data;
3857
3858 if (!conn)
3859 conn = l2cap_conn_add(hcon, 0);
3860
3861 if (!conn)
3862 goto drop;
3863
3864 BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
3865
3866 if (!(flags & ACL_CONT)) {
3867 struct l2cap_hdr *hdr;
3868 struct sock *sk;
3869 u16 cid;
3870 int len;
3871
3872 if (conn->rx_len) {
3873 BT_ERR("Unexpected start frame (len %d)", skb->len);
3874 kfree_skb(conn->rx_skb);
3875 conn->rx_skb = NULL;
3876 conn->rx_len = 0;
3877 l2cap_conn_unreliable(conn, ECOMM);
3878 }
3879
3880 /* Start fragment always begin with Basic L2CAP header */
3881 if (skb->len < L2CAP_HDR_SIZE) {
3882 BT_ERR("Frame is too short (len %d)", skb->len);
3883 l2cap_conn_unreliable(conn, ECOMM);
3884 goto drop;
3885 }
3886
3887 hdr = (struct l2cap_hdr *) skb->data;
3888 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
3889 cid = __le16_to_cpu(hdr->cid);
3890
3891 if (len == skb->len) {
3892 /* Complete frame received */
3893 l2cap_recv_frame(conn, skb);
3894 return 0;
3895 }
3896
3897 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
3898
3899 if (skb->len > len) {
3900 BT_ERR("Frame is too long (len %d, expected len %d)",
3901 skb->len, len);
3902 l2cap_conn_unreliable(conn, ECOMM);
3903 goto drop;
3904 }
3905
3906 sk = l2cap_get_chan_by_scid(&conn->chan_list, cid);
3907
3908 if (sk && l2cap_pi(sk)->imtu < len - L2CAP_HDR_SIZE) {
3909 BT_ERR("Frame exceeding recv MTU (len %d, MTU %d)",
3910 len, l2cap_pi(sk)->imtu);
3911 bh_unlock_sock(sk);
3912 l2cap_conn_unreliable(conn, ECOMM);
3913 goto drop;
3914 }
3915
3916 if (sk)
3917 bh_unlock_sock(sk);
3918
3919 /* Allocate skb for the complete frame (with header) */
3920 conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC);
3921 if (!conn->rx_skb)
3922 goto drop;
3923
3924 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
3925 skb->len);
3926 conn->rx_len = len - skb->len;
3927 } else {
3928 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
3929
3930 if (!conn->rx_len) {
3931 BT_ERR("Unexpected continuation frame (len %d)", skb->len);
3932 l2cap_conn_unreliable(conn, ECOMM);
3933 goto drop;
3934 }
3935
3936 if (skb->len > conn->rx_len) {
3937 BT_ERR("Fragment is too long (len %d, expected %d)",
3938 skb->len, conn->rx_len);
3939 kfree_skb(conn->rx_skb);
3940 conn->rx_skb = NULL;
3941 conn->rx_len = 0;
3942 l2cap_conn_unreliable(conn, ECOMM);
3943 goto drop;
3944 }
3945
3946 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
3947 skb->len);
3948 conn->rx_len -= skb->len;
3949
3950 if (!conn->rx_len) {
3951 /* Complete frame received */
3952 l2cap_recv_frame(conn, conn->rx_skb);
3953 conn->rx_skb = NULL;
3954 }
3955 }
3956
3957 drop:
3958 kfree_skb(skb);
3959 return 0;
3960 }
3961
3962 static int l2cap_debugfs_show(struct seq_file *f, void *p)
3963 {
3964 struct sock *sk;
3965 struct hlist_node *node;
3966
3967 read_lock_bh(&l2cap_sk_list.lock);
3968
3969 sk_for_each(sk, node, &l2cap_sk_list.head) {
3970 struct l2cap_pinfo *pi = l2cap_pi(sk);
3971
3972 seq_printf(f, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n",
3973 batostr(&bt_sk(sk)->src),
3974 batostr(&bt_sk(sk)->dst),
3975 sk->sk_state, __le16_to_cpu(pi->psm),
3976 pi->scid, pi->dcid,
3977 pi->imtu, pi->omtu, pi->sec_level,
3978 pi->mode);
3979 }
3980
3981 read_unlock_bh(&l2cap_sk_list.lock);
3982
3983 return 0;
3984 }
3985
3986 static int l2cap_debugfs_open(struct inode *inode, struct file *file)
3987 {
3988 return single_open(file, l2cap_debugfs_show, inode->i_private);
3989 }
3990
3991 static const struct file_operations l2cap_debugfs_fops = {
3992 .open = l2cap_debugfs_open,
3993 .read = seq_read,
3994 .llseek = seq_lseek,
3995 .release = single_release,
3996 };
3997
3998 static struct dentry *l2cap_debugfs;
3999
4000 static struct hci_proto l2cap_hci_proto = {
4001 .name = "L2CAP",
4002 .id = HCI_PROTO_L2CAP,
4003 .connect_ind = l2cap_connect_ind,
4004 .connect_cfm = l2cap_connect_cfm,
4005 .disconn_ind = l2cap_disconn_ind,
4006 .disconn_cfm = l2cap_disconn_cfm,
4007 .security_cfm = l2cap_security_cfm,
4008 .recv_acldata = l2cap_recv_acldata
4009 };
4010
4011 int __init l2cap_init(void)
4012 {
4013 int err;
4014
4015 err = l2cap_init_sockets();
4016 if (err < 0)
4017 return err;
4018
4019 _busy_wq = create_singlethread_workqueue("l2cap");
4020 if (!_busy_wq) {
4021 err = -ENOMEM;
4022 goto error;
4023 }
4024
4025 err = hci_register_proto(&l2cap_hci_proto);
4026 if (err < 0) {
4027 BT_ERR("L2CAP protocol registration failed");
4028 bt_sock_unregister(BTPROTO_L2CAP);
4029 goto error;
4030 }
4031
4032 if (bt_debugfs) {
4033 l2cap_debugfs = debugfs_create_file("l2cap", 0444,
4034 bt_debugfs, NULL, &l2cap_debugfs_fops);
4035 if (!l2cap_debugfs)
4036 BT_ERR("Failed to create L2CAP debug file");
4037 }
4038
4039 return 0;
4040
4041 error:
4042 destroy_workqueue(_busy_wq);
4043 l2cap_cleanup_sockets();
4044 return err;
4045 }
4046
4047 void l2cap_exit(void)
4048 {
4049 debugfs_remove(l2cap_debugfs);
4050
4051 flush_workqueue(_busy_wq);
4052 destroy_workqueue(_busy_wq);
4053
4054 if (hci_unregister_proto(&l2cap_hci_proto) < 0)
4055 BT_ERR("L2CAP protocol unregistration failed");
4056
4057 l2cap_cleanup_sockets();
4058 }
4059
4060 module_param(disable_ertm, bool, 0644);
4061 MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode");
Linus' kernel tree