nfsd4: typo logical vs bitwise negate for want_mask
[linux-btrfs-devel.git] / net / bluetooth / bnep / core.c
blobca39fcf010ce3353e73312f4172f32c9ce12cb1f
1 /*
2 BNEP implementation for Linux Bluetooth stack (BlueZ).
3 Copyright (C) 2001-2002 Inventel Systemes
4 Written 2001-2002 by
5 Clément Moreau <clement.moreau@inventel.fr>
6 David Libault <david.libault@inventel.fr>
8 Copyright (C) 2002 Maxim Krasnyansky <maxk@qualcomm.com>
10 This program is free software; you can redistribute it and/or modify
11 it under the terms of the GNU General Public License version 2 as
12 published by the Free Software Foundation;
14 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
15 OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
16 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS.
17 IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY
18 CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES
19 WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
20 ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
21 OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
23 ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS,
24 COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS
25 SOFTWARE IS DISCLAIMED.
28 #include <linux/module.h>
30 #include <linux/kernel.h>
31 #include <linux/sched.h>
32 #include <linux/signal.h>
33 #include <linux/init.h>
34 #include <linux/wait.h>
35 #include <linux/freezer.h>
36 #include <linux/errno.h>
37 #include <linux/net.h>
38 #include <linux/slab.h>
39 #include <linux/kthread.h>
40 #include <net/sock.h>
42 #include <linux/socket.h>
43 #include <linux/file.h>
45 #include <linux/netdevice.h>
46 #include <linux/etherdevice.h>
47 #include <linux/skbuff.h>
49 #include <asm/unaligned.h>
51 #include <net/bluetooth/bluetooth.h>
52 #include <net/bluetooth/hci_core.h>
53 #include <net/bluetooth/l2cap.h>
55 #include "bnep.h"
57 #define VERSION "1.3"
59 static int compress_src = 1;
60 static int compress_dst = 1;
62 static LIST_HEAD(bnep_session_list);
63 static DECLARE_RWSEM(bnep_session_sem);
65 static struct bnep_session *__bnep_get_session(u8 *dst)
67 struct bnep_session *s;
68 struct list_head *p;
70 BT_DBG("");
72 list_for_each(p, &bnep_session_list) {
73 s = list_entry(p, struct bnep_session, list);
74 if (!compare_ether_addr(dst, s->eh.h_source))
75 return s;
77 return NULL;
80 static void __bnep_link_session(struct bnep_session *s)
82 /* It's safe to call __module_get() here because sessions are added
83 by the socket layer which has to hold the reference to this module.
85 __module_get(THIS_MODULE);
86 list_add(&s->list, &bnep_session_list);
89 static void __bnep_unlink_session(struct bnep_session *s)
91 list_del(&s->list);
92 module_put(THIS_MODULE);
95 static int bnep_send(struct bnep_session *s, void *data, size_t len)
97 struct socket *sock = s->sock;
98 struct kvec iv = { data, len };
100 return kernel_sendmsg(sock, &s->msg, &iv, 1, len);
103 static int bnep_send_rsp(struct bnep_session *s, u8 ctrl, u16 resp)
105 struct bnep_control_rsp rsp;
106 rsp.type = BNEP_CONTROL;
107 rsp.ctrl = ctrl;
108 rsp.resp = htons(resp);
109 return bnep_send(s, &rsp, sizeof(rsp));
112 #ifdef CONFIG_BT_BNEP_PROTO_FILTER
113 static inline void bnep_set_default_proto_filter(struct bnep_session *s)
115 /* (IPv4, ARP) */
116 s->proto_filter[0].start = ETH_P_IP;
117 s->proto_filter[0].end = ETH_P_ARP;
118 /* (RARP, AppleTalk) */
119 s->proto_filter[1].start = ETH_P_RARP;
120 s->proto_filter[1].end = ETH_P_AARP;
121 /* (IPX, IPv6) */
122 s->proto_filter[2].start = ETH_P_IPX;
123 s->proto_filter[2].end = ETH_P_IPV6;
125 #endif
127 static int bnep_ctrl_set_netfilter(struct bnep_session *s, __be16 *data, int len)
129 int n;
131 if (len < 2)
132 return -EILSEQ;
134 n = get_unaligned_be16(data);
135 data++;
136 len -= 2;
138 if (len < n)
139 return -EILSEQ;
141 BT_DBG("filter len %d", n);
143 #ifdef CONFIG_BT_BNEP_PROTO_FILTER
144 n /= 4;
145 if (n <= BNEP_MAX_PROTO_FILTERS) {
146 struct bnep_proto_filter *f = s->proto_filter;
147 int i;
149 for (i = 0; i < n; i++) {
150 f[i].start = get_unaligned_be16(data++);
151 f[i].end = get_unaligned_be16(data++);
153 BT_DBG("proto filter start %d end %d",
154 f[i].start, f[i].end);
157 if (i < BNEP_MAX_PROTO_FILTERS)
158 memset(f + i, 0, sizeof(*f));
160 if (n == 0)
161 bnep_set_default_proto_filter(s);
163 bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_SUCCESS);
164 } else {
165 bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_FILTER_LIMIT_REACHED);
167 #else
168 bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_FILTER_UNSUPPORTED_REQ);
169 #endif
170 return 0;
173 static int bnep_ctrl_set_mcfilter(struct bnep_session *s, u8 *data, int len)
175 int n;
177 if (len < 2)
178 return -EILSEQ;
180 n = get_unaligned_be16(data);
181 data += 2;
182 len -= 2;
184 if (len < n)
185 return -EILSEQ;
187 BT_DBG("filter len %d", n);
189 #ifdef CONFIG_BT_BNEP_MC_FILTER
190 n /= (ETH_ALEN * 2);
192 if (n > 0) {
193 int i;
195 s->mc_filter = 0;
197 /* Always send broadcast */
198 set_bit(bnep_mc_hash(s->dev->broadcast), (ulong *) &s->mc_filter);
200 /* Add address ranges to the multicast hash */
201 for (; n > 0; n--) {
202 u8 a1[6], *a2;
204 memcpy(a1, data, ETH_ALEN);
205 data += ETH_ALEN;
206 a2 = data;
207 data += ETH_ALEN;
209 BT_DBG("mc filter %s -> %s",
210 batostr((void *) a1), batostr((void *) a2));
212 /* Iterate from a1 to a2 */
213 set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter);
214 while (memcmp(a1, a2, 6) < 0 && s->mc_filter != ~0LL) {
215 /* Increment a1 */
216 i = 5;
217 while (i >= 0 && ++a1[i--] == 0)
220 set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter);
225 BT_DBG("mc filter hash 0x%llx", s->mc_filter);
227 bnep_send_rsp(s, BNEP_FILTER_MULTI_ADDR_RSP, BNEP_SUCCESS);
228 #else
229 bnep_send_rsp(s, BNEP_FILTER_MULTI_ADDR_RSP, BNEP_FILTER_UNSUPPORTED_REQ);
230 #endif
231 return 0;
234 static int bnep_rx_control(struct bnep_session *s, void *data, int len)
236 u8 cmd = *(u8 *)data;
237 int err = 0;
239 data++;
240 len--;
242 switch (cmd) {
243 case BNEP_CMD_NOT_UNDERSTOOD:
244 case BNEP_SETUP_CONN_RSP:
245 case BNEP_FILTER_NET_TYPE_RSP:
246 case BNEP_FILTER_MULTI_ADDR_RSP:
247 /* Ignore these for now */
248 break;
250 case BNEP_FILTER_NET_TYPE_SET:
251 err = bnep_ctrl_set_netfilter(s, data, len);
252 break;
254 case BNEP_FILTER_MULTI_ADDR_SET:
255 err = bnep_ctrl_set_mcfilter(s, data, len);
256 break;
258 case BNEP_SETUP_CONN_REQ:
259 err = bnep_send_rsp(s, BNEP_SETUP_CONN_RSP, BNEP_CONN_NOT_ALLOWED);
260 break;
262 default: {
263 u8 pkt[3];
264 pkt[0] = BNEP_CONTROL;
265 pkt[1] = BNEP_CMD_NOT_UNDERSTOOD;
266 pkt[2] = cmd;
267 bnep_send(s, pkt, sizeof(pkt));
269 break;
272 return err;
275 static int bnep_rx_extension(struct bnep_session *s, struct sk_buff *skb)
277 struct bnep_ext_hdr *h;
278 int err = 0;
280 do {
281 h = (void *) skb->data;
282 if (!skb_pull(skb, sizeof(*h))) {
283 err = -EILSEQ;
284 break;
287 BT_DBG("type 0x%x len %d", h->type, h->len);
289 switch (h->type & BNEP_TYPE_MASK) {
290 case BNEP_EXT_CONTROL:
291 bnep_rx_control(s, skb->data, skb->len);
292 break;
294 default:
295 /* Unknown extension, skip it. */
296 break;
299 if (!skb_pull(skb, h->len)) {
300 err = -EILSEQ;
301 break;
303 } while (!err && (h->type & BNEP_EXT_HEADER));
305 return err;
308 static u8 __bnep_rx_hlen[] = {
309 ETH_HLEN, /* BNEP_GENERAL */
310 0, /* BNEP_CONTROL */
311 2, /* BNEP_COMPRESSED */
312 ETH_ALEN + 2, /* BNEP_COMPRESSED_SRC_ONLY */
313 ETH_ALEN + 2 /* BNEP_COMPRESSED_DST_ONLY */
316 static inline int bnep_rx_frame(struct bnep_session *s, struct sk_buff *skb)
318 struct net_device *dev = s->dev;
319 struct sk_buff *nskb;
320 u8 type;
322 dev->stats.rx_bytes += skb->len;
324 type = *(u8 *) skb->data;
325 skb_pull(skb, 1);
327 if ((type & BNEP_TYPE_MASK) >= sizeof(__bnep_rx_hlen))
328 goto badframe;
330 if ((type & BNEP_TYPE_MASK) == BNEP_CONTROL) {
331 bnep_rx_control(s, skb->data, skb->len);
332 kfree_skb(skb);
333 return 0;
336 skb_reset_mac_header(skb);
338 /* Verify and pull out header */
339 if (!skb_pull(skb, __bnep_rx_hlen[type & BNEP_TYPE_MASK]))
340 goto badframe;
342 s->eh.h_proto = get_unaligned((__be16 *) (skb->data - 2));
344 if (type & BNEP_EXT_HEADER) {
345 if (bnep_rx_extension(s, skb) < 0)
346 goto badframe;
349 /* Strip 802.1p header */
350 if (ntohs(s->eh.h_proto) == 0x8100) {
351 if (!skb_pull(skb, 4))
352 goto badframe;
353 s->eh.h_proto = get_unaligned((__be16 *) (skb->data - 2));
356 /* We have to alloc new skb and copy data here :(. Because original skb
357 * may not be modified and because of the alignment requirements. */
358 nskb = alloc_skb(2 + ETH_HLEN + skb->len, GFP_KERNEL);
359 if (!nskb) {
360 dev->stats.rx_dropped++;
361 kfree_skb(skb);
362 return -ENOMEM;
364 skb_reserve(nskb, 2);
366 /* Decompress header and construct ether frame */
367 switch (type & BNEP_TYPE_MASK) {
368 case BNEP_COMPRESSED:
369 memcpy(__skb_put(nskb, ETH_HLEN), &s->eh, ETH_HLEN);
370 break;
372 case BNEP_COMPRESSED_SRC_ONLY:
373 memcpy(__skb_put(nskb, ETH_ALEN), s->eh.h_dest, ETH_ALEN);
374 memcpy(__skb_put(nskb, ETH_ALEN), skb_mac_header(skb), ETH_ALEN);
375 put_unaligned(s->eh.h_proto, (__be16 *) __skb_put(nskb, 2));
376 break;
378 case BNEP_COMPRESSED_DST_ONLY:
379 memcpy(__skb_put(nskb, ETH_ALEN), skb_mac_header(skb),
380 ETH_ALEN);
381 memcpy(__skb_put(nskb, ETH_ALEN + 2), s->eh.h_source,
382 ETH_ALEN + 2);
383 break;
385 case BNEP_GENERAL:
386 memcpy(__skb_put(nskb, ETH_ALEN * 2), skb_mac_header(skb),
387 ETH_ALEN * 2);
388 put_unaligned(s->eh.h_proto, (__be16 *) __skb_put(nskb, 2));
389 break;
392 skb_copy_from_linear_data(skb, __skb_put(nskb, skb->len), skb->len);
393 kfree_skb(skb);
395 dev->stats.rx_packets++;
396 nskb->ip_summed = CHECKSUM_NONE;
397 nskb->protocol = eth_type_trans(nskb, dev);
398 netif_rx_ni(nskb);
399 return 0;
401 badframe:
402 dev->stats.rx_errors++;
403 kfree_skb(skb);
404 return 0;
407 static u8 __bnep_tx_types[] = {
408 BNEP_GENERAL,
409 BNEP_COMPRESSED_SRC_ONLY,
410 BNEP_COMPRESSED_DST_ONLY,
411 BNEP_COMPRESSED
414 static inline int bnep_tx_frame(struct bnep_session *s, struct sk_buff *skb)
416 struct ethhdr *eh = (void *) skb->data;
417 struct socket *sock = s->sock;
418 struct kvec iv[3];
419 int len = 0, il = 0;
420 u8 type = 0;
422 BT_DBG("skb %p dev %p type %d", skb, skb->dev, skb->pkt_type);
424 if (!skb->dev) {
425 /* Control frame sent by us */
426 goto send;
429 iv[il++] = (struct kvec) { &type, 1 };
430 len++;
432 if (compress_src && !compare_ether_addr(eh->h_dest, s->eh.h_source))
433 type |= 0x01;
435 if (compress_dst && !compare_ether_addr(eh->h_source, s->eh.h_dest))
436 type |= 0x02;
438 if (type)
439 skb_pull(skb, ETH_ALEN * 2);
441 type = __bnep_tx_types[type];
442 switch (type) {
443 case BNEP_COMPRESSED_SRC_ONLY:
444 iv[il++] = (struct kvec) { eh->h_source, ETH_ALEN };
445 len += ETH_ALEN;
446 break;
448 case BNEP_COMPRESSED_DST_ONLY:
449 iv[il++] = (struct kvec) { eh->h_dest, ETH_ALEN };
450 len += ETH_ALEN;
451 break;
454 send:
455 iv[il++] = (struct kvec) { skb->data, skb->len };
456 len += skb->len;
458 /* FIXME: linearize skb */
460 len = kernel_sendmsg(sock, &s->msg, iv, il, len);
462 kfree_skb(skb);
464 if (len > 0) {
465 s->dev->stats.tx_bytes += len;
466 s->dev->stats.tx_packets++;
467 return 0;
470 return len;
473 static int bnep_session(void *arg)
475 struct bnep_session *s = arg;
476 struct net_device *dev = s->dev;
477 struct sock *sk = s->sock->sk;
478 struct sk_buff *skb;
479 wait_queue_t wait;
481 BT_DBG("");
483 set_user_nice(current, -15);
485 init_waitqueue_entry(&wait, current);
486 add_wait_queue(sk_sleep(sk), &wait);
487 while (!kthread_should_stop()) {
488 set_current_state(TASK_INTERRUPTIBLE);
490 /* RX */
491 while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
492 skb_orphan(skb);
493 bnep_rx_frame(s, skb);
496 if (sk->sk_state != BT_CONNECTED)
497 break;
499 /* TX */
500 while ((skb = skb_dequeue(&sk->sk_write_queue)))
501 if (bnep_tx_frame(s, skb))
502 break;
503 netif_wake_queue(dev);
505 schedule();
507 set_current_state(TASK_RUNNING);
508 remove_wait_queue(sk_sleep(sk), &wait);
510 /* Cleanup session */
511 down_write(&bnep_session_sem);
513 /* Delete network device */
514 unregister_netdev(dev);
516 /* Wakeup user-space polling for socket errors */
517 s->sock->sk->sk_err = EUNATCH;
519 wake_up_interruptible(sk_sleep(s->sock->sk));
521 /* Release the socket */
522 fput(s->sock->file);
524 __bnep_unlink_session(s);
526 up_write(&bnep_session_sem);
527 free_netdev(dev);
528 return 0;
531 static struct device *bnep_get_device(struct bnep_session *session)
533 bdaddr_t *src = &bt_sk(session->sock->sk)->src;
534 bdaddr_t *dst = &bt_sk(session->sock->sk)->dst;
535 struct hci_dev *hdev;
536 struct hci_conn *conn;
538 hdev = hci_get_route(dst, src);
539 if (!hdev)
540 return NULL;
542 conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, dst);
544 hci_dev_put(hdev);
546 return conn ? &conn->dev : NULL;
549 static struct device_type bnep_type = {
550 .name = "bluetooth",
553 int bnep_add_connection(struct bnep_connadd_req *req, struct socket *sock)
555 struct net_device *dev;
556 struct bnep_session *s, *ss;
557 u8 dst[ETH_ALEN], src[ETH_ALEN];
558 int err;
560 BT_DBG("");
562 baswap((void *) dst, &bt_sk(sock->sk)->dst);
563 baswap((void *) src, &bt_sk(sock->sk)->src);
565 /* session struct allocated as private part of net_device */
566 dev = alloc_netdev(sizeof(struct bnep_session),
567 (*req->device) ? req->device : "bnep%d",
568 bnep_net_setup);
569 if (!dev)
570 return -ENOMEM;
572 down_write(&bnep_session_sem);
574 ss = __bnep_get_session(dst);
575 if (ss && ss->state == BT_CONNECTED) {
576 err = -EEXIST;
577 goto failed;
580 s = netdev_priv(dev);
582 /* This is rx header therefore addresses are swapped.
583 * ie. eh.h_dest is our local address. */
584 memcpy(s->eh.h_dest, &src, ETH_ALEN);
585 memcpy(s->eh.h_source, &dst, ETH_ALEN);
586 memcpy(dev->dev_addr, s->eh.h_dest, ETH_ALEN);
588 s->dev = dev;
589 s->sock = sock;
590 s->role = req->role;
591 s->state = BT_CONNECTED;
593 s->msg.msg_flags = MSG_NOSIGNAL;
595 #ifdef CONFIG_BT_BNEP_MC_FILTER
596 /* Set default mc filter */
597 set_bit(bnep_mc_hash(dev->broadcast), (ulong *) &s->mc_filter);
598 #endif
600 #ifdef CONFIG_BT_BNEP_PROTO_FILTER
601 /* Set default protocol filter */
602 bnep_set_default_proto_filter(s);
603 #endif
605 SET_NETDEV_DEV(dev, bnep_get_device(s));
606 SET_NETDEV_DEVTYPE(dev, &bnep_type);
608 err = register_netdev(dev);
609 if (err)
610 goto failed;
612 __bnep_link_session(s);
614 s->task = kthread_run(bnep_session, s, "kbnepd %s", dev->name);
615 if (IS_ERR(s->task)) {
616 /* Session thread start failed, gotta cleanup. */
617 unregister_netdev(dev);
618 __bnep_unlink_session(s);
619 err = PTR_ERR(s->task);
620 goto failed;
623 up_write(&bnep_session_sem);
624 strcpy(req->device, dev->name);
625 return 0;
627 failed:
628 up_write(&bnep_session_sem);
629 free_netdev(dev);
630 return err;
633 int bnep_del_connection(struct bnep_conndel_req *req)
635 struct bnep_session *s;
636 int err = 0;
638 BT_DBG("");
640 down_read(&bnep_session_sem);
642 s = __bnep_get_session(req->dst);
643 if (s)
644 kthread_stop(s->task);
645 else
646 err = -ENOENT;
648 up_read(&bnep_session_sem);
649 return err;
652 static void __bnep_copy_ci(struct bnep_conninfo *ci, struct bnep_session *s)
654 memset(ci, 0, sizeof(*ci));
655 memcpy(ci->dst, s->eh.h_source, ETH_ALEN);
656 strcpy(ci->device, s->dev->name);
657 ci->flags = s->flags;
658 ci->state = s->state;
659 ci->role = s->role;
662 int bnep_get_connlist(struct bnep_connlist_req *req)
664 struct list_head *p;
665 int err = 0, n = 0;
667 down_read(&bnep_session_sem);
669 list_for_each(p, &bnep_session_list) {
670 struct bnep_session *s;
671 struct bnep_conninfo ci;
673 s = list_entry(p, struct bnep_session, list);
675 __bnep_copy_ci(&ci, s);
677 if (copy_to_user(req->ci, &ci, sizeof(ci))) {
678 err = -EFAULT;
679 break;
682 if (++n >= req->cnum)
683 break;
685 req->ci++;
687 req->cnum = n;
689 up_read(&bnep_session_sem);
690 return err;
693 int bnep_get_conninfo(struct bnep_conninfo *ci)
695 struct bnep_session *s;
696 int err = 0;
698 down_read(&bnep_session_sem);
700 s = __bnep_get_session(ci->dst);
701 if (s)
702 __bnep_copy_ci(ci, s);
703 else
704 err = -ENOENT;
706 up_read(&bnep_session_sem);
707 return err;
710 static int __init bnep_init(void)
712 char flt[50] = "";
714 #ifdef CONFIG_BT_BNEP_PROTO_FILTER
715 strcat(flt, "protocol ");
716 #endif
718 #ifdef CONFIG_BT_BNEP_MC_FILTER
719 strcat(flt, "multicast");
720 #endif
722 BT_INFO("BNEP (Ethernet Emulation) ver %s", VERSION);
723 if (flt[0])
724 BT_INFO("BNEP filters: %s", flt);
726 bnep_sock_init();
727 return 0;
730 static void __exit bnep_exit(void)
732 bnep_sock_cleanup();
735 module_init(bnep_init);
736 module_exit(bnep_exit);
738 module_param(compress_src, bool, 0644);
739 MODULE_PARM_DESC(compress_src, "Compress sources headers");
741 module_param(compress_dst, bool, 0644);
742 MODULE_PARM_DESC(compress_dst, "Compress destination headers");
744 MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
745 MODULE_DESCRIPTION("Bluetooth BNEP ver " VERSION);
746 MODULE_VERSION(VERSION);
747 MODULE_LICENSE("GPL");
748 MODULE_ALIAS("bt-proto-4");