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