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