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