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