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