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