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