?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
[PATCH] radix tree: early termination of tag clearing
[linux-2.6/zen-sources.git] / net / bluetooth / l2cap.c
blob7f0781e4326f0cb944d6f43bc121eb9f3b695a49
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/config.h>
28 #include <linux/module.h>
29
30 #include <linux/types.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 <net/sock.h>
44
45 #include <asm/system.h>
46 #include <asm/uaccess.h>
47 #include <asm/unaligned.h>
48
49 #include <net/bluetooth/bluetooth.h>
50 #include <net/bluetooth/hci_core.h>
51 #include <net/bluetooth/l2cap.h>
52
53 #ifndef CONFIG_BT_L2CAP_DEBUG
54 #undef BT_DBG
55 #define BT_DBG(D...)
56 #endif
57
58 #define VERSION "2.8"
59
60 static const struct proto_ops l2cap_sock_ops;
61
62 static struct bt_sock_list l2cap_sk_list = {
63 .lock = RW_LOCK_UNLOCKED
64 };
65
66 static int l2cap_conn_del(struct hci_conn *conn, int err);
67
68 static void __l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct sock *parent);
69 static void l2cap_chan_del(struct sock *sk, int err);
70
71 static void __l2cap_sock_close(struct sock *sk, int reason);
72 static void l2cap_sock_close(struct sock *sk);
73 static void l2cap_sock_kill(struct sock *sk);
74
75 static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
76 u8 code, u8 ident, u16 dlen, void *data);
77
78 /* ---- L2CAP timers ---- */
79 static void l2cap_sock_timeout(unsigned long arg)
80 {
81 struct sock *sk = (struct sock *) arg;
82
83 BT_DBG("sock %p state %d", sk, sk->sk_state);
84
85 bh_lock_sock(sk);
86 __l2cap_sock_close(sk, ETIMEDOUT);
87 bh_unlock_sock(sk);
88
89 l2cap_sock_kill(sk);
90 sock_put(sk);
91 }
92
93 static void l2cap_sock_set_timer(struct sock *sk, long timeout)
94 {
95 BT_DBG("sk %p state %d timeout %ld", sk, sk->sk_state, timeout);
96 sk_reset_timer(sk, &sk->sk_timer, jiffies + timeout);
97 }
98
99 static void l2cap_sock_clear_timer(struct sock *sk)
100 {
101 BT_DBG("sock %p state %d", sk, sk->sk_state);
102 sk_stop_timer(sk, &sk->sk_timer);
103 }
104
105 static void l2cap_sock_init_timer(struct sock *sk)
106 {
107 init_timer(&sk->sk_timer);
108 sk->sk_timer.function = l2cap_sock_timeout;
109 sk->sk_timer.data = (unsigned long)sk;
110 }
111
112 /* ---- L2CAP connections ---- */
113 static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status)
114 {
115 struct l2cap_conn *conn;
116
117 if ((conn = hcon->l2cap_data))
118 return conn;
119
120 if (status)
121 return conn;
122
123 if (!(conn = kmalloc(sizeof(struct l2cap_conn), GFP_ATOMIC)))
124 return NULL;
125 memset(conn, 0, sizeof(struct l2cap_conn));
126
127 hcon->l2cap_data = conn;
128 conn->hcon = hcon;
129
130 conn->mtu = hcon->hdev->acl_mtu;
131 conn->src = &hcon->hdev->bdaddr;
132 conn->dst = &hcon->dst;
133
134 spin_lock_init(&conn->lock);
135 rwlock_init(&conn->chan_list.lock);
136
137 BT_DBG("hcon %p conn %p", hcon, conn);
138 return conn;
139 }
140
141 static int l2cap_conn_del(struct hci_conn *hcon, int err)
142 {
143 struct l2cap_conn *conn;
144 struct sock *sk;
145
146 if (!(conn = hcon->l2cap_data))
147 return 0;
148
149 BT_DBG("hcon %p conn %p, err %d", hcon, conn, err);
150
151 if (conn->rx_skb)
152 kfree_skb(conn->rx_skb);
153
154 /* Kill channels */
155 while ((sk = conn->chan_list.head)) {
156 bh_lock_sock(sk);
157 l2cap_chan_del(sk, err);
158 bh_unlock_sock(sk);
159 l2cap_sock_kill(sk);
160 }
161
162 hcon->l2cap_data = NULL;
163 kfree(conn);
164 return 0;
165 }
166
167 static inline void l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct sock *parent)
168 {
169 struct l2cap_chan_list *l = &conn->chan_list;
170 write_lock(&l->lock);
171 __l2cap_chan_add(conn, sk, parent);
172 write_unlock(&l->lock);
173 }
174
175 static inline u8 l2cap_get_ident(struct l2cap_conn *conn)
176 {
177 u8 id;
178
179 /* Get next available identificator.
180 * 1 - 128 are used by kernel.
181 * 129 - 199 are reserved.
182 * 200 - 254 are used by utilities like l2ping, etc.
183 */
184
185 spin_lock(&conn->lock);
186
187 if (++conn->tx_ident > 128)
188 conn->tx_ident = 1;
189
190 id = conn->tx_ident;
191
192 spin_unlock(&conn->lock);
193
194 return id;
195 }
196
197 static inline int l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, void *data)
198 {
199 struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data);
200
201 BT_DBG("code 0x%2.2x", code);
202
203 if (!skb)
204 return -ENOMEM;
205
206 return hci_send_acl(conn->hcon, skb, 0);
207 }
208
209 /* ---- Socket interface ---- */
210 static struct sock *__l2cap_get_sock_by_addr(u16 psm, bdaddr_t *src)
211 {
212 struct sock *sk;
213 struct hlist_node *node;
214 sk_for_each(sk, node, &l2cap_sk_list.head)
215 if (l2cap_pi(sk)->sport == psm && !bacmp(&bt_sk(sk)->src, src))
216 goto found;
217 sk = NULL;
218 found:
219 return sk;
220 }
221
222 /* Find socket with psm and source bdaddr.
223 * Returns closest match.
224 */
225 static struct sock *__l2cap_get_sock_by_psm(int state, u16 psm, bdaddr_t *src)
226 {
227 struct sock *sk = NULL, *sk1 = NULL;
228 struct hlist_node *node;
229
230 sk_for_each(sk, node, &l2cap_sk_list.head) {
231 if (state && sk->sk_state != state)
232 continue;
233
234 if (l2cap_pi(sk)->psm == psm) {
235 /* Exact match. */
236 if (!bacmp(&bt_sk(sk)->src, src))
237 break;
238
239 /* Closest match */
240 if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY))
241 sk1 = sk;
242 }
243 }
244 return node ? sk : sk1;
245 }
246
247 /* Find socket with given address (psm, src).
248 * Returns locked socket */
249 static inline struct sock *l2cap_get_sock_by_psm(int state, u16 psm, bdaddr_t *src)
250 {
251 struct sock *s;
252 read_lock(&l2cap_sk_list.lock);
253 s = __l2cap_get_sock_by_psm(state, psm, src);
254 if (s) bh_lock_sock(s);
255 read_unlock(&l2cap_sk_list.lock);
256 return s;
257 }
258
259 static void l2cap_sock_destruct(struct sock *sk)
260 {
261 BT_DBG("sk %p", sk);
262
263 skb_queue_purge(&sk->sk_receive_queue);
264 skb_queue_purge(&sk->sk_write_queue);
265 }
266
267 static void l2cap_sock_cleanup_listen(struct sock *parent)
268 {
269 struct sock *sk;
270
271 BT_DBG("parent %p", parent);
272
273 /* Close not yet accepted channels */
274 while ((sk = bt_accept_dequeue(parent, NULL)))
275 l2cap_sock_close(sk);
276
277 parent->sk_state = BT_CLOSED;
278 sock_set_flag(parent, SOCK_ZAPPED);
279 }
280
281 /* Kill socket (only if zapped and orphan)
282 * Must be called on unlocked socket.
283 */
284 static void l2cap_sock_kill(struct sock *sk)
285 {
286 if (!sock_flag(sk, SOCK_ZAPPED) || sk->sk_socket)
287 return;
288
289 BT_DBG("sk %p state %d", sk, sk->sk_state);
290
291 /* Kill poor orphan */
292 bt_sock_unlink(&l2cap_sk_list, sk);
293 sock_set_flag(sk, SOCK_DEAD);
294 sock_put(sk);
295 }
296
297 static void __l2cap_sock_close(struct sock *sk, int reason)
298 {
299 BT_DBG("sk %p state %d socket %p", sk, sk->sk_state, sk->sk_socket);
300
301 switch (sk->sk_state) {
302 case BT_LISTEN:
303 l2cap_sock_cleanup_listen(sk);
304 break;
305
306 case BT_CONNECTED:
307 case BT_CONFIG:
308 case BT_CONNECT2:
309 if (sk->sk_type == SOCK_SEQPACKET) {
310 struct l2cap_conn *conn = l2cap_pi(sk)->conn;
311 struct l2cap_disconn_req req;
312
313 sk->sk_state = BT_DISCONN;
314 l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
315
316 req.dcid = __cpu_to_le16(l2cap_pi(sk)->dcid);
317 req.scid = __cpu_to_le16(l2cap_pi(sk)->scid);
318 l2cap_send_cmd(conn, l2cap_get_ident(conn),
319 L2CAP_DISCONN_REQ, sizeof(req), &req);
320 } else {
321 l2cap_chan_del(sk, reason);
322 }
323 break;
324
325 case BT_CONNECT:
326 case BT_DISCONN:
327 l2cap_chan_del(sk, reason);
328 break;
329
330 default:
331 sock_set_flag(sk, SOCK_ZAPPED);
332 break;
333 }
334 }
335
336 /* Must be called on unlocked socket. */
337 static void l2cap_sock_close(struct sock *sk)
338 {
339 l2cap_sock_clear_timer(sk);
340 lock_sock(sk);
341 __l2cap_sock_close(sk, ECONNRESET);
342 release_sock(sk);
343 l2cap_sock_kill(sk);
344 }
345
346 static void l2cap_sock_init(struct sock *sk, struct sock *parent)
347 {
348 struct l2cap_pinfo *pi = l2cap_pi(sk);
349
350 BT_DBG("sk %p", sk);
351
352 if (parent) {
353 sk->sk_type = parent->sk_type;
354 pi->imtu = l2cap_pi(parent)->imtu;
355 pi->omtu = l2cap_pi(parent)->omtu;
356 pi->link_mode = l2cap_pi(parent)->link_mode;
357 } else {
358 pi->imtu = L2CAP_DEFAULT_MTU;
359 pi->omtu = 0;
360 pi->link_mode = 0;
361 }
362
363 /* Default config options */
364 pi->conf_mtu = L2CAP_DEFAULT_MTU;
365 pi->flush_to = L2CAP_DEFAULT_FLUSH_TO;
366 }
367
368 static struct proto l2cap_proto = {
369 .name = "L2CAP",
370 .owner = THIS_MODULE,
371 .obj_size = sizeof(struct l2cap_pinfo)
372 };
373
374 static struct sock *l2cap_sock_alloc(struct socket *sock, int proto, gfp_t prio)
375 {
376 struct sock *sk;
377
378 sk = sk_alloc(PF_BLUETOOTH, prio, &l2cap_proto, 1);
379 if (!sk)
380 return NULL;
381
382 sock_init_data(sock, sk);
383 INIT_LIST_HEAD(&bt_sk(sk)->accept_q);
384
385 sk->sk_destruct = l2cap_sock_destruct;
386 sk->sk_sndtimeo = L2CAP_CONN_TIMEOUT;
387
388 sock_reset_flag(sk, SOCK_ZAPPED);
389
390 sk->sk_protocol = proto;
391 sk->sk_state = BT_OPEN;
392
393 l2cap_sock_init_timer(sk);
394
395 bt_sock_link(&l2cap_sk_list, sk);
396 return sk;
397 }
398
399 static int l2cap_sock_create(struct socket *sock, int protocol)
400 {
401 struct sock *sk;
402
403 BT_DBG("sock %p", sock);
404
405 sock->state = SS_UNCONNECTED;
406
407 if (sock->type != SOCK_SEQPACKET &&
408 sock->type != SOCK_DGRAM && sock->type != SOCK_RAW)
409 return -ESOCKTNOSUPPORT;
410
411 if (sock->type == SOCK_RAW && !capable(CAP_NET_RAW))
412 return -EPERM;
413
414 sock->ops = &l2cap_sock_ops;
415
416 sk = l2cap_sock_alloc(sock, protocol, GFP_KERNEL);
417 if (!sk)
418 return -ENOMEM;
419
420 l2cap_sock_init(sk, NULL);
421 return 0;
422 }
423
424 static int l2cap_sock_bind(struct socket *sock, struct sockaddr *addr, int addr_len)
425 {
426 struct sockaddr_l2 *la = (struct sockaddr_l2 *) addr;
427 struct sock *sk = sock->sk;
428 int err = 0;
429
430 BT_DBG("sk %p, %s %d", sk, batostr(&la->l2_bdaddr), la->l2_psm);
431
432 if (!addr || addr->sa_family != AF_BLUETOOTH)
433 return -EINVAL;
434
435 lock_sock(sk);
436
437 if (sk->sk_state != BT_OPEN) {
438 err = -EBADFD;
439 goto done;
440 }
441
442 write_lock_bh(&l2cap_sk_list.lock);
443
444 if (la->l2_psm && __l2cap_get_sock_by_addr(la->l2_psm, &la->l2_bdaddr)) {
445 err = -EADDRINUSE;
446 } else {
447 /* Save source address */
448 bacpy(&bt_sk(sk)->src, &la->l2_bdaddr);
449 l2cap_pi(sk)->psm = la->l2_psm;
450 l2cap_pi(sk)->sport = la->l2_psm;
451 sk->sk_state = BT_BOUND;
452 }
453
454 write_unlock_bh(&l2cap_sk_list.lock);
455
456 done:
457 release_sock(sk);
458 return err;
459 }
460
461 static int l2cap_do_connect(struct sock *sk)
462 {
463 bdaddr_t *src = &bt_sk(sk)->src;
464 bdaddr_t *dst = &bt_sk(sk)->dst;
465 struct l2cap_conn *conn;
466 struct hci_conn *hcon;
467 struct hci_dev *hdev;
468 int err = 0;
469
470 BT_DBG("%s -> %s psm 0x%2.2x", batostr(src), batostr(dst), l2cap_pi(sk)->psm);
471
472 if (!(hdev = hci_get_route(dst, src)))
473 return -EHOSTUNREACH;
474
475 hci_dev_lock_bh(hdev);
476
477 err = -ENOMEM;
478
479 hcon = hci_connect(hdev, ACL_LINK, dst);
480 if (!hcon)
481 goto done;
482
483 conn = l2cap_conn_add(hcon, 0);
484 if (!conn) {
485 hci_conn_put(hcon);
486 goto done;
487 }
488
489 err = 0;
490
491 /* Update source addr of the socket */
492 bacpy(src, conn->src);
493
494 l2cap_chan_add(conn, sk, NULL);
495
496 sk->sk_state = BT_CONNECT;
497 l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
498
499 if (hcon->state == BT_CONNECTED) {
500 if (sk->sk_type == SOCK_SEQPACKET) {
501 struct l2cap_conn_req req;
502 l2cap_pi(sk)->ident = l2cap_get_ident(conn);
503 req.scid = __cpu_to_le16(l2cap_pi(sk)->scid);
504 req.psm = l2cap_pi(sk)->psm;
505 l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
506 L2CAP_CONN_REQ, sizeof(req), &req);
507 } else {
508 l2cap_sock_clear_timer(sk);
509 sk->sk_state = BT_CONNECTED;
510 }
511 }
512
513 done:
514 hci_dev_unlock_bh(hdev);
515 hci_dev_put(hdev);
516 return err;
517 }
518
519 static int l2cap_sock_connect(struct socket *sock, struct sockaddr *addr, int alen, int flags)
520 {
521 struct sockaddr_l2 *la = (struct sockaddr_l2 *) addr;
522 struct sock *sk = sock->sk;
523 int err = 0;
524
525 lock_sock(sk);
526
527 BT_DBG("sk %p", sk);
528
529 if (addr->sa_family != AF_BLUETOOTH || alen < sizeof(struct sockaddr_l2)) {
530 err = -EINVAL;
531 goto done;
532 }
533
534 if (sk->sk_type == SOCK_SEQPACKET && !la->l2_psm) {
535 err = -EINVAL;
536 goto done;
537 }
538
539 switch(sk->sk_state) {
540 case BT_CONNECT:
541 case BT_CONNECT2:
542 case BT_CONFIG:
543 /* Already connecting */
544 goto wait;
545
546 case BT_CONNECTED:
547 /* Already connected */
548 goto done;
549
550 case BT_OPEN:
551 case BT_BOUND:
552 /* Can connect */
553 break;
554
555 default:
556 err = -EBADFD;
557 goto done;
558 }
559
560 /* Set destination address and psm */
561 bacpy(&bt_sk(sk)->dst, &la->l2_bdaddr);
562 l2cap_pi(sk)->psm = la->l2_psm;
563
564 if ((err = l2cap_do_connect(sk)))
565 goto done;
566
567 wait:
568 err = bt_sock_wait_state(sk, BT_CONNECTED,
569 sock_sndtimeo(sk, flags & O_NONBLOCK));
570 done:
571 release_sock(sk);
572 return err;
573 }
574
575 static int l2cap_sock_listen(struct socket *sock, int backlog)
576 {
577 struct sock *sk = sock->sk;
578 int err = 0;
579
580 BT_DBG("sk %p backlog %d", sk, backlog);
581
582 lock_sock(sk);
583
584 if (sk->sk_state != BT_BOUND || sock->type != SOCK_SEQPACKET) {
585 err = -EBADFD;
586 goto done;
587 }
588
589 if (!l2cap_pi(sk)->psm) {
590 bdaddr_t *src = &bt_sk(sk)->src;
591 u16 psm;
592
593 err = -EINVAL;
594
595 write_lock_bh(&l2cap_sk_list.lock);
596
597 for (psm = 0x1001; psm < 0x1100; psm += 2)
598 if (!__l2cap_get_sock_by_addr(psm, src)) {
599 l2cap_pi(sk)->psm = htobs(psm);
600 l2cap_pi(sk)->sport = htobs(psm);
601 err = 0;
602 break;
603 }
604
605 write_unlock_bh(&l2cap_sk_list.lock);
606
607 if (err < 0)
608 goto done;
609 }
610
611 sk->sk_max_ack_backlog = backlog;
612 sk->sk_ack_backlog = 0;
613 sk->sk_state = BT_LISTEN;
614
615 done:
616 release_sock(sk);
617 return err;
618 }
619
620 static int l2cap_sock_accept(struct socket *sock, struct socket *newsock, int flags)
621 {
622 DECLARE_WAITQUEUE(wait, current);
623 struct sock *sk = sock->sk, *nsk;
624 long timeo;
625 int err = 0;
626
627 lock_sock(sk);
628
629 if (sk->sk_state != BT_LISTEN) {
630 err = -EBADFD;
631 goto done;
632 }
633
634 timeo = sock_rcvtimeo(sk, flags & O_NONBLOCK);
635
636 BT_DBG("sk %p timeo %ld", sk, timeo);
637
638 /* Wait for an incoming connection. (wake-one). */
639 add_wait_queue_exclusive(sk->sk_sleep, &wait);
640 while (!(nsk = bt_accept_dequeue(sk, newsock))) {
641 set_current_state(TASK_INTERRUPTIBLE);
642 if (!timeo) {
643 err = -EAGAIN;
644 break;
645 }
646
647 release_sock(sk);
648 timeo = schedule_timeout(timeo);
649 lock_sock(sk);
650
651 if (sk->sk_state != BT_LISTEN) {
652 err = -EBADFD;
653 break;
654 }
655
656 if (signal_pending(current)) {
657 err = sock_intr_errno(timeo);
658 break;
659 }
660 }
661 set_current_state(TASK_RUNNING);
662 remove_wait_queue(sk->sk_sleep, &wait);
663
664 if (err)
665 goto done;
666
667 newsock->state = SS_CONNECTED;
668
669 BT_DBG("new socket %p", nsk);
670
671 done:
672 release_sock(sk);
673 return err;
674 }
675
676 static int l2cap_sock_getname(struct socket *sock, struct sockaddr *addr, int *len, int peer)
677 {
678 struct sockaddr_l2 *la = (struct sockaddr_l2 *) addr;
679 struct sock *sk = sock->sk;
680
681 BT_DBG("sock %p, sk %p", sock, sk);
682
683 addr->sa_family = AF_BLUETOOTH;
684 *len = sizeof(struct sockaddr_l2);
685
686 if (peer)
687 bacpy(&la->l2_bdaddr, &bt_sk(sk)->dst);
688 else
689 bacpy(&la->l2_bdaddr, &bt_sk(sk)->src);
690
691 la->l2_psm = l2cap_pi(sk)->psm;
692 return 0;
693 }
694
695 static inline int l2cap_do_send(struct sock *sk, struct msghdr *msg, int len)
696 {
697 struct l2cap_conn *conn = l2cap_pi(sk)->conn;
698 struct sk_buff *skb, **frag;
699 int err, hlen, count, sent=0;
700 struct l2cap_hdr *lh;
701
702 BT_DBG("sk %p len %d", sk, len);
703
704 /* First fragment (with L2CAP header) */
705 if (sk->sk_type == SOCK_DGRAM)
706 hlen = L2CAP_HDR_SIZE + 2;
707 else
708 hlen = L2CAP_HDR_SIZE;
709
710 count = min_t(unsigned int, (conn->mtu - hlen), len);
711
712 skb = bt_skb_send_alloc(sk, hlen + count,
713 msg->msg_flags & MSG_DONTWAIT, &err);
714 if (!skb)
715 return err;
716
717 /* Create L2CAP header */
718 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
719 lh->cid = __cpu_to_le16(l2cap_pi(sk)->dcid);
720 lh->len = __cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
721
722 if (sk->sk_type == SOCK_DGRAM)
723 put_unaligned(l2cap_pi(sk)->psm, (u16 *) skb_put(skb, 2));
724
725 if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count)) {
726 err = -EFAULT;
727 goto fail;
728 }
729
730 sent += count;
731 len -= count;
732
733 /* Continuation fragments (no L2CAP header) */
734 frag = &skb_shinfo(skb)->frag_list;
735 while (len) {
736 count = min_t(unsigned int, conn->mtu, len);
737
738 *frag = bt_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err);
739 if (!*frag)
740 goto fail;
741
742 if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count)) {
743 err = -EFAULT;
744 goto fail;
745 }
746
747 sent += count;
748 len -= count;
749
750 frag = &(*frag)->next;
751 }
752
753 if ((err = hci_send_acl(conn->hcon, skb, 0)) < 0)
754 goto fail;
755
756 return sent;
757
758 fail:
759 kfree_skb(skb);
760 return err;
761 }
762
763 static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len)
764 {
765 struct sock *sk = sock->sk;
766 int err = 0;
767
768 BT_DBG("sock %p, sk %p", sock, sk);
769
770 err = sock_error(sk);
771 if (err)
772 return err;
773
774 if (msg->msg_flags & MSG_OOB)
775 return -EOPNOTSUPP;
776
777 /* Check outgoing MTU */
778 if (sk->sk_type != SOCK_RAW && len > l2cap_pi(sk)->omtu)
779 return -EINVAL;
780
781 lock_sock(sk);
782
783 if (sk->sk_state == BT_CONNECTED)
784 err = l2cap_do_send(sk, msg, len);
785 else
786 err = -ENOTCONN;
787
788 release_sock(sk);
789 return err;
790 }
791
792 static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname, char __user *optval, int optlen)
793 {
794 struct sock *sk = sock->sk;
795 struct l2cap_options opts;
796 int err = 0, len;
797 u32 opt;
798
799 BT_DBG("sk %p", sk);
800
801 lock_sock(sk);
802
803 switch (optname) {
804 case L2CAP_OPTIONS:
805 len = min_t(unsigned int, sizeof(opts), optlen);
806 if (copy_from_user((char *) &opts, optval, len)) {
807 err = -EFAULT;
808 break;
809 }
810 l2cap_pi(sk)->imtu = opts.imtu;
811 l2cap_pi(sk)->omtu = opts.omtu;
812 break;
813
814 case L2CAP_LM:
815 if (get_user(opt, (u32 __user *) optval)) {
816 err = -EFAULT;
817 break;
818 }
819
820 l2cap_pi(sk)->link_mode = opt;
821 break;
822
823 default:
824 err = -ENOPROTOOPT;
825 break;
826 }
827
828 release_sock(sk);
829 return err;
830 }
831
832 static int l2cap_sock_getsockopt(struct socket *sock, int level, int optname, char __user *optval, int __user *optlen)
833 {
834 struct sock *sk = sock->sk;
835 struct l2cap_options opts;
836 struct l2cap_conninfo cinfo;
837 int len, err = 0;
838
839 BT_DBG("sk %p", sk);
840
841 if (get_user(len, optlen))
842 return -EFAULT;
843
844 lock_sock(sk);
845
846 switch (optname) {
847 case L2CAP_OPTIONS:
848 opts.imtu = l2cap_pi(sk)->imtu;
849 opts.omtu = l2cap_pi(sk)->omtu;
850 opts.flush_to = l2cap_pi(sk)->flush_to;
851 opts.mode = 0x00;
852
853 len = min_t(unsigned int, len, sizeof(opts));
854 if (copy_to_user(optval, (char *) &opts, len))
855 err = -EFAULT;
856
857 break;
858
859 case L2CAP_LM:
860 if (put_user(l2cap_pi(sk)->link_mode, (u32 __user *) optval))
861 err = -EFAULT;
862 break;
863
864 case L2CAP_CONNINFO:
865 if (sk->sk_state != BT_CONNECTED) {
866 err = -ENOTCONN;
867 break;
868 }
869
870 cinfo.hci_handle = l2cap_pi(sk)->conn->hcon->handle;
871 memcpy(cinfo.dev_class, l2cap_pi(sk)->conn->hcon->dev_class, 3);
872
873 len = min_t(unsigned int, len, sizeof(cinfo));
874 if (copy_to_user(optval, (char *) &cinfo, len))
875 err = -EFAULT;
876
877 break;
878
879 default:
880 err = -ENOPROTOOPT;
881 break;
882 }
883
884 release_sock(sk);
885 return err;
886 }
887
888 static int l2cap_sock_shutdown(struct socket *sock, int how)
889 {
890 struct sock *sk = sock->sk;
891 int err = 0;
892
893 BT_DBG("sock %p, sk %p", sock, sk);
894
895 if (!sk)
896 return 0;
897
898 lock_sock(sk);
899 if (!sk->sk_shutdown) {
900 sk->sk_shutdown = SHUTDOWN_MASK;
901 l2cap_sock_clear_timer(sk);
902 __l2cap_sock_close(sk, 0);
903
904 if (sock_flag(sk, SOCK_LINGER) && sk->sk_lingertime)
905 err = bt_sock_wait_state(sk, BT_CLOSED, sk->sk_lingertime);
906 }
907 release_sock(sk);
908 return err;
909 }
910
911 static int l2cap_sock_release(struct socket *sock)
912 {
913 struct sock *sk = sock->sk;
914 int err;
915
916 BT_DBG("sock %p, sk %p", sock, sk);
917
918 if (!sk)
919 return 0;
920
921 err = l2cap_sock_shutdown(sock, 2);
922
923 sock_orphan(sk);
924 l2cap_sock_kill(sk);
925 return err;
926 }
927
928 /* ---- L2CAP channels ---- */
929 static struct sock *__l2cap_get_chan_by_dcid(struct l2cap_chan_list *l, u16 cid)
930 {
931 struct sock *s;
932 for (s = l->head; s; s = l2cap_pi(s)->next_c) {
933 if (l2cap_pi(s)->dcid == cid)
934 break;
935 }
936 return s;
937 }
938
939 static struct sock *__l2cap_get_chan_by_scid(struct l2cap_chan_list *l, u16 cid)
940 {
941 struct sock *s;
942 for (s = l->head; s; s = l2cap_pi(s)->next_c) {
943 if (l2cap_pi(s)->scid == cid)
944 break;
945 }
946 return s;
947 }
948
949 /* Find channel with given SCID.
950 * Returns locked socket */
951 static inline struct sock *l2cap_get_chan_by_scid(struct l2cap_chan_list *l, u16 cid)
952 {
953 struct sock *s;
954 read_lock(&l->lock);
955 s = __l2cap_get_chan_by_scid(l, cid);
956 if (s) bh_lock_sock(s);
957 read_unlock(&l->lock);
958 return s;
959 }
960
961 static struct sock *__l2cap_get_chan_by_ident(struct l2cap_chan_list *l, u8 ident)
962 {
963 struct sock *s;
964 for (s = l->head; s; s = l2cap_pi(s)->next_c) {
965 if (l2cap_pi(s)->ident == ident)
966 break;
967 }
968 return s;
969 }
970
971 static inline struct sock *l2cap_get_chan_by_ident(struct l2cap_chan_list *l, u8 ident)
972 {
973 struct sock *s;
974 read_lock(&l->lock);
975 s = __l2cap_get_chan_by_ident(l, ident);
976 if (s) bh_lock_sock(s);
977 read_unlock(&l->lock);
978 return s;
979 }
980
981 static u16 l2cap_alloc_cid(struct l2cap_chan_list *l)
982 {
983 u16 cid = 0x0040;
984
985 for (; cid < 0xffff; cid++) {
986 if(!__l2cap_get_chan_by_scid(l, cid))
987 return cid;
988 }
989
990 return 0;
991 }
992
993 static inline void __l2cap_chan_link(struct l2cap_chan_list *l, struct sock *sk)
994 {
995 sock_hold(sk);
996
997 if (l->head)
998 l2cap_pi(l->head)->prev_c = sk;
999
1000 l2cap_pi(sk)->next_c = l->head;
1001 l2cap_pi(sk)->prev_c = NULL;
1002 l->head = sk;
1003 }
1004
1005 static inline void l2cap_chan_unlink(struct l2cap_chan_list *l, struct sock *sk)
1006 {
1007 struct sock *next = l2cap_pi(sk)->next_c, *prev = l2cap_pi(sk)->prev_c;
1008
1009 write_lock(&l->lock);
1010 if (sk == l->head)
1011 l->head = next;
1012
1013 if (next)
1014 l2cap_pi(next)->prev_c = prev;
1015 if (prev)
1016 l2cap_pi(prev)->next_c = next;
1017 write_unlock(&l->lock);
1018
1019 __sock_put(sk);
1020 }
1021
1022 static void __l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct sock *parent)
1023 {
1024 struct l2cap_chan_list *l = &conn->chan_list;
1025
1026 BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn, l2cap_pi(sk)->psm, l2cap_pi(sk)->dcid);
1027
1028 l2cap_pi(sk)->conn = conn;
1029
1030 if (sk->sk_type == SOCK_SEQPACKET) {
1031 /* Alloc CID for connection-oriented socket */
1032 l2cap_pi(sk)->scid = l2cap_alloc_cid(l);
1033 } else if (sk->sk_type == SOCK_DGRAM) {
1034 /* Connectionless socket */
1035 l2cap_pi(sk)->scid = 0x0002;
1036 l2cap_pi(sk)->dcid = 0x0002;
1037 l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU;
1038 } else {
1039 /* Raw socket can send/recv signalling messages only */
1040 l2cap_pi(sk)->scid = 0x0001;
1041 l2cap_pi(sk)->dcid = 0x0001;
1042 l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU;
1043 }
1044
1045 __l2cap_chan_link(l, sk);
1046
1047 if (parent)
1048 bt_accept_enqueue(parent, sk);
1049 }
1050
1051 /* Delete channel.
1052 * Must be called on the locked socket. */
1053 static void l2cap_chan_del(struct sock *sk, int err)
1054 {
1055 struct l2cap_conn *conn = l2cap_pi(sk)->conn;
1056 struct sock *parent = bt_sk(sk)->parent;
1057
1058 l2cap_sock_clear_timer(sk);
1059
1060 BT_DBG("sk %p, conn %p, err %d", sk, conn, err);
1061
1062 if (conn) {
1063 /* Unlink from channel list */
1064 l2cap_chan_unlink(&conn->chan_list, sk);
1065 l2cap_pi(sk)->conn = NULL;
1066 hci_conn_put(conn->hcon);
1067 }
1068
1069 sk->sk_state = BT_CLOSED;
1070 sock_set_flag(sk, SOCK_ZAPPED);
1071
1072 if (err)
1073 sk->sk_err = err;
1074
1075 if (parent) {
1076 bt_accept_unlink(sk);
1077 parent->sk_data_ready(parent, 0);
1078 } else
1079 sk->sk_state_change(sk);
1080 }
1081
1082 static void l2cap_conn_ready(struct l2cap_conn *conn)
1083 {
1084 struct l2cap_chan_list *l = &conn->chan_list;
1085 struct sock *sk;
1086
1087 BT_DBG("conn %p", conn);
1088
1089 read_lock(&l->lock);
1090
1091 for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
1092 bh_lock_sock(sk);
1093
1094 if (sk->sk_type != SOCK_SEQPACKET) {
1095 l2cap_sock_clear_timer(sk);
1096 sk->sk_state = BT_CONNECTED;
1097 sk->sk_state_change(sk);
1098 } else if (sk->sk_state == BT_CONNECT) {
1099 struct l2cap_conn_req req;
1100 l2cap_pi(sk)->ident = l2cap_get_ident(conn);
1101 req.scid = __cpu_to_le16(l2cap_pi(sk)->scid);
1102 req.psm = l2cap_pi(sk)->psm;
1103 l2cap_send_cmd(conn, l2cap_pi(sk)->ident, L2CAP_CONN_REQ, sizeof(req), &req);
1104 }
1105
1106 bh_unlock_sock(sk);
1107 }
1108
1109 read_unlock(&l->lock);
1110 }
1111
1112 /* Notify sockets that we cannot guaranty reliability anymore */
1113 static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err)
1114 {
1115 struct l2cap_chan_list *l = &conn->chan_list;
1116 struct sock *sk;
1117
1118 BT_DBG("conn %p", conn);
1119
1120 read_lock(&l->lock);
1121 for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
1122 if (l2cap_pi(sk)->link_mode & L2CAP_LM_RELIABLE)
1123 sk->sk_err = err;
1124 }
1125 read_unlock(&l->lock);
1126 }
1127
1128 static void l2cap_chan_ready(struct sock *sk)
1129 {
1130 struct sock *parent = bt_sk(sk)->parent;
1131
1132 BT_DBG("sk %p, parent %p", sk, parent);
1133
1134 l2cap_pi(sk)->conf_state = 0;
1135 l2cap_sock_clear_timer(sk);
1136
1137 if (!parent) {
1138 /* Outgoing channel.
1139 * Wake up socket sleeping on connect.
1140 */
1141 sk->sk_state = BT_CONNECTED;
1142 sk->sk_state_change(sk);
1143 } else {
1144 /* Incoming channel.
1145 * Wake up socket sleeping on accept.
1146 */
1147 parent->sk_data_ready(parent, 0);
1148 }
1149 }
1150
1151 /* Copy frame to all raw sockets on that connection */
1152 static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb)
1153 {
1154 struct l2cap_chan_list *l = &conn->chan_list;
1155 struct sk_buff *nskb;
1156 struct sock * sk;
1157
1158 BT_DBG("conn %p", conn);
1159
1160 read_lock(&l->lock);
1161 for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
1162 if (sk->sk_type != SOCK_RAW)
1163 continue;
1164
1165 /* Don't send frame to the socket it came from */
1166 if (skb->sk == sk)
1167 continue;
1168
1169 if (!(nskb = skb_clone(skb, GFP_ATOMIC)))
1170 continue;
1171
1172 if (sock_queue_rcv_skb(sk, nskb))
1173 kfree_skb(nskb);
1174 }
1175 read_unlock(&l->lock);
1176 }
1177
1178 /* ---- L2CAP signalling commands ---- */
1179 static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
1180 u8 code, u8 ident, u16 dlen, void *data)
1181 {
1182 struct sk_buff *skb, **frag;
1183 struct l2cap_cmd_hdr *cmd;
1184 struct l2cap_hdr *lh;
1185 int len, count;
1186
1187 BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %d", conn, code, ident, dlen);
1188
1189 len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen;
1190 count = min_t(unsigned int, conn->mtu, len);
1191
1192 skb = bt_skb_alloc(count, GFP_ATOMIC);
1193 if (!skb)
1194 return NULL;
1195
1196 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1197 lh->len = __cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen);
1198 lh->cid = __cpu_to_le16(0x0001);
1199
1200 cmd = (struct l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE);
1201 cmd->code = code;
1202 cmd->ident = ident;
1203 cmd->len = __cpu_to_le16(dlen);
1204
1205 if (dlen) {
1206 count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE;
1207 memcpy(skb_put(skb, count), data, count);
1208 data += count;
1209 }
1210
1211 len -= skb->len;
1212
1213 /* Continuation fragments (no L2CAP header) */
1214 frag = &skb_shinfo(skb)->frag_list;
1215 while (len) {
1216 count = min_t(unsigned int, conn->mtu, len);
1217
1218 *frag = bt_skb_alloc(count, GFP_ATOMIC);
1219 if (!*frag)
1220 goto fail;
1221
1222 memcpy(skb_put(*frag, count), data, count);
1223
1224 len -= count;
1225 data += count;
1226
1227 frag = &(*frag)->next;
1228 }
1229
1230 return skb;
1231
1232 fail:
1233 kfree_skb(skb);
1234 return NULL;
1235 }
1236
1237 static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, unsigned long *val)
1238 {
1239 struct l2cap_conf_opt *opt = *ptr;
1240 int len;
1241
1242 len = L2CAP_CONF_OPT_SIZE + opt->len;
1243 *ptr += len;
1244
1245 *type = opt->type;
1246 *olen = opt->len;
1247
1248 switch (opt->len) {
1249 case 1:
1250 *val = *((u8 *) opt->val);
1251 break;
1252
1253 case 2:
1254 *val = __le16_to_cpu(*((u16 *)opt->val));
1255 break;
1256
1257 case 4:
1258 *val = __le32_to_cpu(*((u32 *)opt->val));
1259 break;
1260
1261 default:
1262 *val = (unsigned long) opt->val;
1263 break;
1264 }
1265
1266 BT_DBG("type 0x%2.2x len %d val 0x%lx", *type, opt->len, *val);
1267 return len;
1268 }
1269
1270 static inline void l2cap_parse_conf_req(struct sock *sk, void *data, int len)
1271 {
1272 int type, hint, olen;
1273 unsigned long val;
1274 void *ptr = data;
1275
1276 BT_DBG("sk %p len %d", sk, len);
1277
1278 while (len >= L2CAP_CONF_OPT_SIZE) {
1279 len -= l2cap_get_conf_opt(&ptr, &type, &olen, &val);
1280
1281 hint = type & 0x80;
1282 type &= 0x7f;
1283
1284 switch (type) {
1285 case L2CAP_CONF_MTU:
1286 l2cap_pi(sk)->conf_mtu = val;
1287 break;
1288
1289 case L2CAP_CONF_FLUSH_TO:
1290 l2cap_pi(sk)->flush_to = val;
1291 break;
1292
1293 case L2CAP_CONF_QOS:
1294 break;
1295
1296 default:
1297 if (hint)
1298 break;
1299
1300 /* FIXME: Reject unknown option */
1301 break;
1302 }
1303 }
1304 }
1305
1306 static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val)
1307 {
1308 struct l2cap_conf_opt *opt = *ptr;
1309
1310 BT_DBG("type 0x%2.2x len %d val 0x%lx", type, len, val);
1311
1312 opt->type = type;
1313 opt->len = len;
1314
1315 switch (len) {
1316 case 1:
1317 *((u8 *) opt->val) = val;
1318 break;
1319
1320 case 2:
1321 *((u16 *) opt->val) = __cpu_to_le16(val);
1322 break;
1323
1324 case 4:
1325 *((u32 *) opt->val) = __cpu_to_le32(val);
1326 break;
1327
1328 default:
1329 memcpy(opt->val, (void *) val, len);
1330 break;
1331 }
1332
1333 *ptr += L2CAP_CONF_OPT_SIZE + len;
1334 }
1335
1336 static int l2cap_build_conf_req(struct sock *sk, void *data)
1337 {
1338 struct l2cap_pinfo *pi = l2cap_pi(sk);
1339 struct l2cap_conf_req *req = data;
1340 void *ptr = req->data;
1341
1342 BT_DBG("sk %p", sk);
1343
1344 if (pi->imtu != L2CAP_DEFAULT_MTU)
1345 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->imtu);
1346
1347 /* FIXME: Need actual value of the flush timeout */
1348 //if (flush_to != L2CAP_DEFAULT_FLUSH_TO)
1349 // l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO, 2, pi->flush_to);
1350
1351 req->dcid = __cpu_to_le16(pi->dcid);
1352 req->flags = __cpu_to_le16(0);
1353
1354 return ptr - data;
1355 }
1356
1357 static inline int l2cap_conf_output(struct sock *sk, void **ptr)
1358 {
1359 struct l2cap_pinfo *pi = l2cap_pi(sk);
1360 int result = 0;
1361
1362 /* Configure output options and let the other side know
1363 * which ones we don't like. */
1364 if (pi->conf_mtu < pi->omtu) {
1365 l2cap_add_conf_opt(ptr, L2CAP_CONF_MTU, 2, pi->omtu);
1366 result = L2CAP_CONF_UNACCEPT;
1367 } else {
1368 pi->omtu = pi->conf_mtu;
1369 }
1370
1371 BT_DBG("sk %p result %d", sk, result);
1372 return result;
1373 }
1374
1375 static int l2cap_build_conf_rsp(struct sock *sk, void *data, int *result)
1376 {
1377 struct l2cap_conf_rsp *rsp = data;
1378 void *ptr = rsp->data;
1379 u16 flags = 0;
1380
1381 BT_DBG("sk %p complete %d", sk, result ? 1 : 0);
1382
1383 if (result)
1384 *result = l2cap_conf_output(sk, &ptr);
1385 else
1386 flags = 0x0001;
1387
1388 rsp->scid = __cpu_to_le16(l2cap_pi(sk)->dcid);
1389 rsp->result = __cpu_to_le16(result ? *result : 0);
1390 rsp->flags = __cpu_to_le16(flags);
1391
1392 return ptr - data;
1393 }
1394
1395 static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1396 {
1397 struct l2cap_chan_list *list = &conn->chan_list;
1398 struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
1399 struct l2cap_conn_rsp rsp;
1400 struct sock *sk, *parent;
1401 int result = 0, status = 0;
1402
1403 u16 dcid = 0, scid = __le16_to_cpu(req->scid);
1404 u16 psm = req->psm;
1405
1406 BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid);
1407
1408 /* Check if we have socket listening on psm */
1409 parent = l2cap_get_sock_by_psm(BT_LISTEN, psm, conn->src);
1410 if (!parent) {
1411 result = L2CAP_CR_BAD_PSM;
1412 goto sendresp;
1413 }
1414
1415 result = L2CAP_CR_NO_MEM;
1416
1417 /* Check for backlog size */
1418 if (sk_acceptq_is_full(parent)) {
1419 BT_DBG("backlog full %d", parent->sk_ack_backlog);
1420 goto response;
1421 }
1422
1423 sk = l2cap_sock_alloc(NULL, BTPROTO_L2CAP, GFP_ATOMIC);
1424 if (!sk)
1425 goto response;
1426
1427 write_lock(&list->lock);
1428
1429 /* Check if we already have channel with that dcid */
1430 if (__l2cap_get_chan_by_dcid(list, scid)) {
1431 write_unlock(&list->lock);
1432 sock_set_flag(sk, SOCK_ZAPPED);
1433 l2cap_sock_kill(sk);
1434 goto response;
1435 }
1436
1437 hci_conn_hold(conn->hcon);
1438
1439 l2cap_sock_init(sk, parent);
1440 bacpy(&bt_sk(sk)->src, conn->src);
1441 bacpy(&bt_sk(sk)->dst, conn->dst);
1442 l2cap_pi(sk)->psm = psm;
1443 l2cap_pi(sk)->dcid = scid;
1444
1445 __l2cap_chan_add(conn, sk, parent);
1446 dcid = l2cap_pi(sk)->scid;
1447
1448 l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
1449
1450 /* Service level security */
1451 result = L2CAP_CR_PEND;
1452 status = L2CAP_CS_AUTHEN_PEND;
1453 sk->sk_state = BT_CONNECT2;
1454 l2cap_pi(sk)->ident = cmd->ident;
1455
1456 if ((l2cap_pi(sk)->link_mode & L2CAP_LM_ENCRYPT) ||
1457 (l2cap_pi(sk)->link_mode & L2CAP_LM_SECURE)) {
1458 if (!hci_conn_encrypt(conn->hcon))
1459 goto done;
1460 } else if (l2cap_pi(sk)->link_mode & L2CAP_LM_AUTH) {
1461 if (!hci_conn_auth(conn->hcon))
1462 goto done;
1463 }
1464
1465 sk->sk_state = BT_CONFIG;
1466 result = status = 0;
1467
1468 done:
1469 write_unlock(&list->lock);
1470
1471 response:
1472 bh_unlock_sock(parent);
1473
1474 sendresp:
1475 rsp.scid = __cpu_to_le16(scid);
1476 rsp.dcid = __cpu_to_le16(dcid);
1477 rsp.result = __cpu_to_le16(result);
1478 rsp.status = __cpu_to_le16(status);
1479 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
1480 return 0;
1481 }
1482
1483 static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1484 {
1485 struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
1486 u16 scid, dcid, result, status;
1487 struct sock *sk;
1488 u8 req[128];
1489
1490 scid = __le16_to_cpu(rsp->scid);
1491 dcid = __le16_to_cpu(rsp->dcid);
1492 result = __le16_to_cpu(rsp->result);
1493 status = __le16_to_cpu(rsp->status);
1494
1495 BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", dcid, scid, result, status);
1496
1497 if (scid) {
1498 if (!(sk = l2cap_get_chan_by_scid(&conn->chan_list, scid)))
1499 return 0;
1500 } else {
1501 if (!(sk = l2cap_get_chan_by_ident(&conn->chan_list, cmd->ident)))
1502 return 0;
1503 }
1504
1505 switch (result) {
1506 case L2CAP_CR_SUCCESS:
1507 sk->sk_state = BT_CONFIG;
1508 l2cap_pi(sk)->ident = 0;
1509 l2cap_pi(sk)->dcid = dcid;
1510 l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
1511
1512 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
1513 l2cap_build_conf_req(sk, req), req);
1514 break;
1515
1516 case L2CAP_CR_PEND:
1517 break;
1518
1519 default:
1520 l2cap_chan_del(sk, ECONNREFUSED);
1521 break;
1522 }
1523
1524 bh_unlock_sock(sk);
1525 return 0;
1526 }
1527
1528 static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1529 {
1530 struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
1531 u16 dcid, flags;
1532 u8 rsp[64];
1533 struct sock *sk;
1534 int result;
1535
1536 dcid = __le16_to_cpu(req->dcid);
1537 flags = __le16_to_cpu(req->flags);
1538
1539 BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
1540
1541 if (!(sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid)))
1542 return -ENOENT;
1543
1544 l2cap_parse_conf_req(sk, req->data, cmd->len - sizeof(*req));
1545
1546 if (flags & 0x0001) {
1547 /* Incomplete config. Send empty response. */
1548 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
1549 l2cap_build_conf_rsp(sk, rsp, NULL), rsp);
1550 goto unlock;
1551 }
1552
1553 /* Complete config. */
1554 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
1555 l2cap_build_conf_rsp(sk, rsp, &result), rsp);
1556
1557 if (result)
1558 goto unlock;
1559
1560 /* Output config done */
1561 l2cap_pi(sk)->conf_state |= L2CAP_CONF_OUTPUT_DONE;
1562
1563 if (l2cap_pi(sk)->conf_state & L2CAP_CONF_INPUT_DONE) {
1564 sk->sk_state = BT_CONNECTED;
1565 l2cap_chan_ready(sk);
1566 } else if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT)) {
1567 u8 req[64];
1568 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
1569 l2cap_build_conf_req(sk, req), req);
1570 }
1571
1572 unlock:
1573 bh_unlock_sock(sk);
1574 return 0;
1575 }
1576
1577 static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1578 {
1579 struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
1580 u16 scid, flags, result;
1581 struct sock *sk;
1582
1583 scid = __le16_to_cpu(rsp->scid);
1584 flags = __le16_to_cpu(rsp->flags);
1585 result = __le16_to_cpu(rsp->result);
1586
1587 BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x", scid, flags, result);
1588
1589 if (!(sk = l2cap_get_chan_by_scid(&conn->chan_list, scid)))
1590 return 0;
1591
1592 switch (result) {
1593 case L2CAP_CONF_SUCCESS:
1594 break;
1595
1596 case L2CAP_CONF_UNACCEPT:
1597 if (++l2cap_pi(sk)->conf_retry < L2CAP_CONF_MAX_RETRIES) {
1598 char req[128];
1599 /* It does not make sense to adjust L2CAP parameters
1600 * that are currently defined in the spec. We simply
1601 * resend config request that we sent earlier. It is
1602 * stupid, but it helps qualification testing which
1603 * expects at least some response from us. */
1604 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
1605 l2cap_build_conf_req(sk, req), req);
1606 goto done;
1607 }
1608
1609 default:
1610 sk->sk_state = BT_DISCONN;
1611 sk->sk_err = ECONNRESET;
1612 l2cap_sock_set_timer(sk, HZ * 5);
1613 {
1614 struct l2cap_disconn_req req;
1615 req.dcid = __cpu_to_le16(l2cap_pi(sk)->dcid);
1616 req.scid = __cpu_to_le16(l2cap_pi(sk)->scid);
1617 l2cap_send_cmd(conn, l2cap_get_ident(conn),
1618 L2CAP_DISCONN_REQ, sizeof(req), &req);
1619 }
1620 goto done;
1621 }
1622
1623 if (flags & 0x01)
1624 goto done;
1625
1626 /* Input config done */
1627 l2cap_pi(sk)->conf_state |= L2CAP_CONF_INPUT_DONE;
1628
1629 if (l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE) {
1630 sk->sk_state = BT_CONNECTED;
1631 l2cap_chan_ready(sk);
1632 }
1633
1634 done:
1635 bh_unlock_sock(sk);
1636 return 0;
1637 }
1638
1639 static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1640 {
1641 struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
1642 struct l2cap_disconn_rsp rsp;
1643 u16 dcid, scid;
1644 struct sock *sk;
1645
1646 scid = __le16_to_cpu(req->scid);
1647 dcid = __le16_to_cpu(req->dcid);
1648
1649 BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
1650
1651 if (!(sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid)))
1652 return 0;
1653
1654 rsp.dcid = __cpu_to_le16(l2cap_pi(sk)->scid);
1655 rsp.scid = __cpu_to_le16(l2cap_pi(sk)->dcid);
1656 l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
1657
1658 sk->sk_shutdown = SHUTDOWN_MASK;
1659
1660 l2cap_chan_del(sk, ECONNRESET);
1661 bh_unlock_sock(sk);
1662
1663 l2cap_sock_kill(sk);
1664 return 0;
1665 }
1666
1667 static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1668 {
1669 struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
1670 u16 dcid, scid;
1671 struct sock *sk;
1672
1673 scid = __le16_to_cpu(rsp->scid);
1674 dcid = __le16_to_cpu(rsp->dcid);
1675
1676 BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
1677
1678 if (!(sk = l2cap_get_chan_by_scid(&conn->chan_list, scid)))
1679 return 0;
1680
1681 l2cap_chan_del(sk, 0);
1682 bh_unlock_sock(sk);
1683
1684 l2cap_sock_kill(sk);
1685 return 0;
1686 }
1687
1688 static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1689 {
1690 struct l2cap_info_req *req = (struct l2cap_info_req *) data;
1691 struct l2cap_info_rsp rsp;
1692 u16 type;
1693
1694 type = __le16_to_cpu(req->type);
1695
1696 BT_DBG("type 0x%4.4x", type);
1697
1698 rsp.type = __cpu_to_le16(type);
1699 rsp.result = __cpu_to_le16(L2CAP_IR_NOTSUPP);
1700 l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(rsp), &rsp);
1701
1702 return 0;
1703 }
1704
1705 static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
1706 {
1707 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
1708 u16 type, result;
1709
1710 type = __le16_to_cpu(rsp->type);
1711 result = __le16_to_cpu(rsp->result);
1712
1713 BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
1714
1715 return 0;
1716 }
1717
1718 static inline void l2cap_sig_channel(struct l2cap_conn *conn, struct sk_buff *skb)
1719 {
1720 u8 *data = skb->data;
1721 int len = skb->len;
1722 struct l2cap_cmd_hdr cmd;
1723 int err = 0;
1724
1725 l2cap_raw_recv(conn, skb);
1726
1727 while (len >= L2CAP_CMD_HDR_SIZE) {
1728 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
1729 data += L2CAP_CMD_HDR_SIZE;
1730 len -= L2CAP_CMD_HDR_SIZE;
1731
1732 cmd.len = __le16_to_cpu(cmd.len);
1733
1734 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd.len, cmd.ident);
1735
1736 if (cmd.len > len || !cmd.ident) {
1737 BT_DBG("corrupted command");
1738 break;
1739 }
1740
1741 switch (cmd.code) {
1742 case L2CAP_COMMAND_REJ:
1743 /* FIXME: We should process this */
1744 break;
1745
1746 case L2CAP_CONN_REQ:
1747 err = l2cap_connect_req(conn, &cmd, data);
1748 break;
1749
1750 case L2CAP_CONN_RSP:
1751 err = l2cap_connect_rsp(conn, &cmd, data);
1752 break;
1753
1754 case L2CAP_CONF_REQ:
1755 err = l2cap_config_req(conn, &cmd, data);
1756 break;
1757
1758 case L2CAP_CONF_RSP:
1759 err = l2cap_config_rsp(conn, &cmd, data);
1760 break;
1761
1762 case L2CAP_DISCONN_REQ:
1763 err = l2cap_disconnect_req(conn, &cmd, data);
1764 break;
1765
1766 case L2CAP_DISCONN_RSP:
1767 err = l2cap_disconnect_rsp(conn, &cmd, data);
1768 break;
1769
1770 case L2CAP_ECHO_REQ:
1771 l2cap_send_cmd(conn, cmd.ident, L2CAP_ECHO_RSP, cmd.len, data);
1772 break;
1773
1774 case L2CAP_ECHO_RSP:
1775 break;
1776
1777 case L2CAP_INFO_REQ:
1778 err = l2cap_information_req(conn, &cmd, data);
1779 break;
1780
1781 case L2CAP_INFO_RSP:
1782 err = l2cap_information_rsp(conn, &cmd, data);
1783 break;
1784
1785 default:
1786 BT_ERR("Unknown signaling command 0x%2.2x", cmd.code);
1787 err = -EINVAL;
1788 break;
1789 }
1790
1791 if (err) {
1792 struct l2cap_cmd_rej rej;
1793 BT_DBG("error %d", err);
1794
1795 /* FIXME: Map err to a valid reason */
1796 rej.reason = __cpu_to_le16(0);
1797 l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
1798 }
1799
1800 data += cmd.len;
1801 len -= cmd.len;
1802 }
1803
1804 kfree_skb(skb);
1805 }
1806
1807 static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
1808 {
1809 struct sock *sk;
1810
1811 sk = l2cap_get_chan_by_scid(&conn->chan_list, cid);
1812 if (!sk) {
1813 BT_DBG("unknown cid 0x%4.4x", cid);
1814 goto drop;
1815 }
1816
1817 BT_DBG("sk %p, len %d", sk, skb->len);
1818
1819 if (sk->sk_state != BT_CONNECTED)
1820 goto drop;
1821
1822 if (l2cap_pi(sk)->imtu < skb->len)
1823 goto drop;
1824
1825 /* If socket recv buffers overflows we drop data here
1826 * which is *bad* because L2CAP has to be reliable.
1827 * But we don't have any other choice. L2CAP doesn't
1828 * provide flow control mechanism. */
1829
1830 if (!sock_queue_rcv_skb(sk, skb))
1831 goto done;
1832
1833 drop:
1834 kfree_skb(skb);
1835
1836 done:
1837 if (sk) bh_unlock_sock(sk);
1838 return 0;
1839 }
1840
1841 static inline int l2cap_conless_channel(struct l2cap_conn *conn, u16 psm, struct sk_buff *skb)
1842 {
1843 struct sock *sk;
1844
1845 sk = l2cap_get_sock_by_psm(0, psm, conn->src);
1846 if (!sk)
1847 goto drop;
1848
1849 BT_DBG("sk %p, len %d", sk, skb->len);
1850
1851 if (sk->sk_state != BT_BOUND && sk->sk_state != BT_CONNECTED)
1852 goto drop;
1853
1854 if (l2cap_pi(sk)->imtu < skb->len)
1855 goto drop;
1856
1857 if (!sock_queue_rcv_skb(sk, skb))
1858 goto done;
1859
1860 drop:
1861 kfree_skb(skb);
1862
1863 done:
1864 if (sk) bh_unlock_sock(sk);
1865 return 0;
1866 }
1867
1868 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
1869 {
1870 struct l2cap_hdr *lh = (void *) skb->data;
1871 u16 cid, psm, len;
1872
1873 skb_pull(skb, L2CAP_HDR_SIZE);
1874 cid = __le16_to_cpu(lh->cid);
1875 len = __le16_to_cpu(lh->len);
1876
1877 BT_DBG("len %d, cid 0x%4.4x", len, cid);
1878
1879 switch (cid) {
1880 case 0x0001:
1881 l2cap_sig_channel(conn, skb);
1882 break;
1883
1884 case 0x0002:
1885 psm = get_unaligned((u16 *) skb->data);
1886 skb_pull(skb, 2);
1887 l2cap_conless_channel(conn, psm, skb);
1888 break;
1889
1890 default:
1891 l2cap_data_channel(conn, cid, skb);
1892 break;
1893 }
1894 }
1895
1896 /* ---- L2CAP interface with lower layer (HCI) ---- */
1897
1898 static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
1899 {
1900 int exact = 0, lm1 = 0, lm2 = 0;
1901 register struct sock *sk;
1902 struct hlist_node *node;
1903
1904 if (type != ACL_LINK)
1905 return 0;
1906
1907 BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
1908
1909 /* Find listening sockets and check their link_mode */
1910 read_lock(&l2cap_sk_list.lock);
1911 sk_for_each(sk, node, &l2cap_sk_list.head) {
1912 if (sk->sk_state != BT_LISTEN)
1913 continue;
1914
1915 if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
1916 lm1 |= (HCI_LM_ACCEPT | l2cap_pi(sk)->link_mode);
1917 exact++;
1918 } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY))
1919 lm2 |= (HCI_LM_ACCEPT | l2cap_pi(sk)->link_mode);
1920 }
1921 read_unlock(&l2cap_sk_list.lock);
1922
1923 return exact ? lm1 : lm2;
1924 }
1925
1926 static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
1927 {
1928 BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
1929
1930 if (hcon->type != ACL_LINK)
1931 return 0;
1932
1933 if (!status) {
1934 struct l2cap_conn *conn;
1935
1936 conn = l2cap_conn_add(hcon, status);
1937 if (conn)
1938 l2cap_conn_ready(conn);
1939 } else
1940 l2cap_conn_del(hcon, bt_err(status));
1941
1942 return 0;
1943 }
1944
1945 static int l2cap_disconn_ind(struct hci_conn *hcon, u8 reason)
1946 {
1947 BT_DBG("hcon %p reason %d", hcon, reason);
1948
1949 if (hcon->type != ACL_LINK)
1950 return 0;
1951
1952 l2cap_conn_del(hcon, bt_err(reason));
1953 return 0;
1954 }
1955
1956 static int l2cap_auth_cfm(struct hci_conn *hcon, u8 status)
1957 {
1958 struct l2cap_chan_list *l;
1959 struct l2cap_conn *conn;
1960 struct l2cap_conn_rsp rsp;
1961 struct sock *sk;
1962 int result;
1963
1964 if (!(conn = hcon->l2cap_data))
1965 return 0;
1966 l = &conn->chan_list;
1967
1968 BT_DBG("conn %p", conn);
1969
1970 read_lock(&l->lock);
1971
1972 for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
1973 bh_lock_sock(sk);
1974
1975 if (sk->sk_state != BT_CONNECT2 ||
1976 (l2cap_pi(sk)->link_mode & L2CAP_LM_ENCRYPT) ||
1977 (l2cap_pi(sk)->link_mode & L2CAP_LM_SECURE)) {
1978 bh_unlock_sock(sk);
1979 continue;
1980 }
1981
1982 if (!status) {
1983 sk->sk_state = BT_CONFIG;
1984 result = 0;
1985 } else {
1986 sk->sk_state = BT_DISCONN;
1987 l2cap_sock_set_timer(sk, HZ/10);
1988 result = L2CAP_CR_SEC_BLOCK;
1989 }
1990
1991 rsp.scid = __cpu_to_le16(l2cap_pi(sk)->dcid);
1992 rsp.dcid = __cpu_to_le16(l2cap_pi(sk)->scid);
1993 rsp.result = __cpu_to_le16(result);
1994 rsp.status = __cpu_to_le16(0);
1995 l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
1996 L2CAP_CONN_RSP, sizeof(rsp), &rsp);
1997
1998 bh_unlock_sock(sk);
1999 }
2000
2001 read_unlock(&l->lock);
2002 return 0;
2003 }
2004
2005 static int l2cap_encrypt_cfm(struct hci_conn *hcon, u8 status)
2006 {
2007 struct l2cap_chan_list *l;
2008 struct l2cap_conn *conn;
2009 struct l2cap_conn_rsp rsp;
2010 struct sock *sk;
2011 int result;
2012
2013 if (!(conn = hcon->l2cap_data))
2014 return 0;
2015 l = &conn->chan_list;
2016
2017 BT_DBG("conn %p", conn);
2018
2019 read_lock(&l->lock);
2020
2021 for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
2022 bh_lock_sock(sk);
2023
2024 if (sk->sk_state != BT_CONNECT2) {
2025 bh_unlock_sock(sk);
2026 continue;
2027 }
2028
2029 if (!status) {
2030 sk->sk_state = BT_CONFIG;
2031 result = 0;
2032 } else {
2033 sk->sk_state = BT_DISCONN;
2034 l2cap_sock_set_timer(sk, HZ/10);
2035 result = L2CAP_CR_SEC_BLOCK;
2036 }
2037
2038 rsp.scid = __cpu_to_le16(l2cap_pi(sk)->dcid);
2039 rsp.dcid = __cpu_to_le16(l2cap_pi(sk)->scid);
2040 rsp.result = __cpu_to_le16(result);
2041 rsp.status = __cpu_to_le16(0);
2042 l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
2043 L2CAP_CONN_RSP, sizeof(rsp), &rsp);
2044
2045 if (l2cap_pi(sk)->link_mode & L2CAP_LM_SECURE)
2046 hci_conn_change_link_key(hcon);
2047
2048 bh_unlock_sock(sk);
2049 }
2050
2051 read_unlock(&l->lock);
2052 return 0;
2053 }
2054
2055 static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
2056 {
2057 struct l2cap_conn *conn = hcon->l2cap_data;
2058
2059 if (!conn && !(conn = l2cap_conn_add(hcon, 0)))
2060 goto drop;
2061
2062 BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
2063
2064 if (flags & ACL_START) {
2065 struct l2cap_hdr *hdr;
2066 int len;
2067
2068 if (conn->rx_len) {
2069 BT_ERR("Unexpected start frame (len %d)", skb->len);
2070 kfree_skb(conn->rx_skb);
2071 conn->rx_skb = NULL;
2072 conn->rx_len = 0;
2073 l2cap_conn_unreliable(conn, ECOMM);
2074 }
2075
2076 if (skb->len < 2) {
2077 BT_ERR("Frame is too short (len %d)", skb->len);
2078 l2cap_conn_unreliable(conn, ECOMM);
2079 goto drop;
2080 }
2081
2082 hdr = (struct l2cap_hdr *) skb->data;
2083 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
2084
2085 if (len == skb->len) {
2086 /* Complete frame received */
2087 l2cap_recv_frame(conn, skb);
2088 return 0;
2089 }
2090
2091 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
2092
2093 if (skb->len > len) {
2094 BT_ERR("Frame is too long (len %d, expected len %d)",
2095 skb->len, len);
2096 l2cap_conn_unreliable(conn, ECOMM);
2097 goto drop;
2098 }
2099
2100 /* Allocate skb for the complete frame (with header) */
2101 if (!(conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC)))
2102 goto drop;
2103
2104 memcpy(skb_put(conn->rx_skb, skb->len), skb->data, skb->len);
2105 conn->rx_len = len - skb->len;
2106 } else {
2107 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
2108
2109 if (!conn->rx_len) {
2110 BT_ERR("Unexpected continuation frame (len %d)", skb->len);
2111 l2cap_conn_unreliable(conn, ECOMM);
2112 goto drop;
2113 }
2114
2115 if (skb->len > conn->rx_len) {
2116 BT_ERR("Fragment is too long (len %d, expected %d)",
2117 skb->len, conn->rx_len);
2118 kfree_skb(conn->rx_skb);
2119 conn->rx_skb = NULL;
2120 conn->rx_len = 0;
2121 l2cap_conn_unreliable(conn, ECOMM);
2122 goto drop;
2123 }
2124
2125 memcpy(skb_put(conn->rx_skb, skb->len), skb->data, skb->len);
2126 conn->rx_len -= skb->len;
2127
2128 if (!conn->rx_len) {
2129 /* Complete frame received */
2130 l2cap_recv_frame(conn, conn->rx_skb);
2131 conn->rx_skb = NULL;
2132 }
2133 }
2134
2135 drop:
2136 kfree_skb(skb);
2137 return 0;
2138 }
2139
2140 static ssize_t l2cap_sysfs_show(struct class *dev, char *buf)
2141 {
2142 struct sock *sk;
2143 struct hlist_node *node;
2144 char *str = buf;
2145
2146 read_lock_bh(&l2cap_sk_list.lock);
2147
2148 sk_for_each(sk, node, &l2cap_sk_list.head) {
2149 struct l2cap_pinfo *pi = l2cap_pi(sk);
2150
2151 str += sprintf(str, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d 0x%x\n",
2152 batostr(&bt_sk(sk)->src), batostr(&bt_sk(sk)->dst),
2153 sk->sk_state, pi->psm, pi->scid, pi->dcid, pi->imtu,
2154 pi->omtu, pi->link_mode);
2155 }
2156
2157 read_unlock_bh(&l2cap_sk_list.lock);
2158
2159 return (str - buf);
2160 }
2161
2162 static CLASS_ATTR(l2cap, S_IRUGO, l2cap_sysfs_show, NULL);
2163
2164 static const struct proto_ops l2cap_sock_ops = {
2165 .family = PF_BLUETOOTH,
2166 .owner = THIS_MODULE,
2167 .release = l2cap_sock_release,
2168 .bind = l2cap_sock_bind,
2169 .connect = l2cap_sock_connect,
2170 .listen = l2cap_sock_listen,
2171 .accept = l2cap_sock_accept,
2172 .getname = l2cap_sock_getname,
2173 .sendmsg = l2cap_sock_sendmsg,
2174 .recvmsg = bt_sock_recvmsg,
2175 .poll = bt_sock_poll,
2176 .mmap = sock_no_mmap,
2177 .socketpair = sock_no_socketpair,
2178 .ioctl = sock_no_ioctl,
2179 .shutdown = l2cap_sock_shutdown,
2180 .setsockopt = l2cap_sock_setsockopt,
2181 .getsockopt = l2cap_sock_getsockopt
2182 };
2183
2184 static struct net_proto_family l2cap_sock_family_ops = {
2185 .family = PF_BLUETOOTH,
2186 .owner = THIS_MODULE,
2187 .create = l2cap_sock_create,
2188 };
2189
2190 static struct hci_proto l2cap_hci_proto = {
2191 .name = "L2CAP",
2192 .id = HCI_PROTO_L2CAP,
2193 .connect_ind = l2cap_connect_ind,
2194 .connect_cfm = l2cap_connect_cfm,
2195 .disconn_ind = l2cap_disconn_ind,
2196 .auth_cfm = l2cap_auth_cfm,
2197 .encrypt_cfm = l2cap_encrypt_cfm,
2198 .recv_acldata = l2cap_recv_acldata
2199 };
2200
2201 static int __init l2cap_init(void)
2202 {
2203 int err;
2204
2205 err = proto_register(&l2cap_proto, 0);
2206 if (err < 0)
2207 return err;
2208
2209 err = bt_sock_register(BTPROTO_L2CAP, &l2cap_sock_family_ops);
2210 if (err < 0) {
2211 BT_ERR("L2CAP socket registration failed");
2212 goto error;
2213 }
2214
2215 err = hci_register_proto(&l2cap_hci_proto);
2216 if (err < 0) {
2217 BT_ERR("L2CAP protocol registration failed");
2218 bt_sock_unregister(BTPROTO_L2CAP);
2219 goto error;
2220 }
2221
2222 class_create_file(&bt_class, &class_attr_l2cap);
2223
2224 BT_INFO("L2CAP ver %s", VERSION);
2225 BT_INFO("L2CAP socket layer initialized");
2226
2227 return 0;
2228
2229 error:
2230 proto_unregister(&l2cap_proto);
2231 return err;
2232 }
2233
2234 static void __exit l2cap_exit(void)
2235 {
2236 class_remove_file(&bt_class, &class_attr_l2cap);
2237
2238 if (bt_sock_unregister(BTPROTO_L2CAP) < 0)
2239 BT_ERR("L2CAP socket unregistration failed");
2240
2241 if (hci_unregister_proto(&l2cap_hci_proto) < 0)
2242 BT_ERR("L2CAP protocol unregistration failed");
2243
2244 proto_unregister(&l2cap_proto);
2245 }
2246
2247 void l2cap_load(void)
2248 {
2249 /* Dummy function to trigger automatic L2CAP module loading by
2250 * other modules that use L2CAP sockets but don't use any other
2251 * symbols from it. */
2252 return;
2253 }
2254 EXPORT_SYMBOL(l2cap_load);
2255
2256 module_init(l2cap_init);
2257 module_exit(l2cap_exit);
2258
2259 MODULE_AUTHOR("Maxim Krasnyansky <maxk@qualcomm.com>, Marcel Holtmann <marcel@holtmann.org>");
2260 MODULE_DESCRIPTION("Bluetooth L2CAP ver " VERSION);
2261 MODULE_VERSION(VERSION);
2262 MODULE_LICENSE("GPL");
2263 MODULE_ALIAS("bt-proto-0");
Stable & featurefull patchset based on Linus's git branch