Merge branch 'akpm'
[linux-2.6/next.git] / net / bluetooth / bnep / core.c
blob91bcd3a961ec22c501451ef588fe86b171c55ae3
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 (1) {
488 set_current_state(TASK_INTERRUPTIBLE);
490 if (atomic_read(&s->terminate))
491 break;
492 /* RX */
493 while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
494 skb_orphan(skb);
495 if (!skb_linearize(skb))
496 bnep_rx_frame(s, skb);
497 else
498 kfree_skb(skb);
501 if (sk->sk_state != BT_CONNECTED)
502 break;
504 /* TX */
505 while ((skb = skb_dequeue(&sk->sk_write_queue)))
506 if (bnep_tx_frame(s, skb))
507 break;
508 netif_wake_queue(dev);
510 schedule();
512 __set_current_state(TASK_RUNNING);
513 remove_wait_queue(sk_sleep(sk), &wait);
515 /* Cleanup session */
516 down_write(&bnep_session_sem);
518 /* Delete network device */
519 unregister_netdev(dev);
521 /* Wakeup user-space polling for socket errors */
522 s->sock->sk->sk_err = EUNATCH;
524 wake_up_interruptible(sk_sleep(s->sock->sk));
526 /* Release the socket */
527 fput(s->sock->file);
529 __bnep_unlink_session(s);
531 up_write(&bnep_session_sem);
532 free_netdev(dev);
533 return 0;
536 static struct device *bnep_get_device(struct bnep_session *session)
538 bdaddr_t *src = &bt_sk(session->sock->sk)->src;
539 bdaddr_t *dst = &bt_sk(session->sock->sk)->dst;
540 struct hci_dev *hdev;
541 struct hci_conn *conn;
543 hdev = hci_get_route(dst, src);
544 if (!hdev)
545 return NULL;
547 conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, dst);
549 hci_dev_put(hdev);
551 return conn ? &conn->dev : NULL;
554 static struct device_type bnep_type = {
555 .name = "bluetooth",
558 int bnep_add_connection(struct bnep_connadd_req *req, struct socket *sock)
560 struct net_device *dev;
561 struct bnep_session *s, *ss;
562 u8 dst[ETH_ALEN], src[ETH_ALEN];
563 int err;
565 BT_DBG("");
567 baswap((void *) dst, &bt_sk(sock->sk)->dst);
568 baswap((void *) src, &bt_sk(sock->sk)->src);
570 /* session struct allocated as private part of net_device */
571 dev = alloc_netdev(sizeof(struct bnep_session),
572 (*req->device) ? req->device : "bnep%d",
573 bnep_net_setup);
574 if (!dev)
575 return -ENOMEM;
577 down_write(&bnep_session_sem);
579 ss = __bnep_get_session(dst);
580 if (ss && ss->state == BT_CONNECTED) {
581 err = -EEXIST;
582 goto failed;
585 s = netdev_priv(dev);
587 /* This is rx header therefore addresses are swapped.
588 * ie. eh.h_dest is our local address. */
589 memcpy(s->eh.h_dest, &src, ETH_ALEN);
590 memcpy(s->eh.h_source, &dst, ETH_ALEN);
591 memcpy(dev->dev_addr, s->eh.h_dest, ETH_ALEN);
593 s->dev = dev;
594 s->sock = sock;
595 s->role = req->role;
596 s->state = BT_CONNECTED;
598 s->msg.msg_flags = MSG_NOSIGNAL;
600 #ifdef CONFIG_BT_BNEP_MC_FILTER
601 /* Set default mc filter */
602 set_bit(bnep_mc_hash(dev->broadcast), (ulong *) &s->mc_filter);
603 #endif
605 #ifdef CONFIG_BT_BNEP_PROTO_FILTER
606 /* Set default protocol filter */
607 bnep_set_default_proto_filter(s);
608 #endif
610 SET_NETDEV_DEV(dev, bnep_get_device(s));
611 SET_NETDEV_DEVTYPE(dev, &bnep_type);
613 err = register_netdev(dev);
614 if (err)
615 goto failed;
617 __bnep_link_session(s);
619 s->task = kthread_run(bnep_session, s, "kbnepd %s", dev->name);
620 if (IS_ERR(s->task)) {
621 /* Session thread start failed, gotta cleanup. */
622 unregister_netdev(dev);
623 __bnep_unlink_session(s);
624 err = PTR_ERR(s->task);
625 goto failed;
628 up_write(&bnep_session_sem);
629 strcpy(req->device, dev->name);
630 return 0;
632 failed:
633 up_write(&bnep_session_sem);
634 free_netdev(dev);
635 return err;
638 int bnep_del_connection(struct bnep_conndel_req *req)
640 struct bnep_session *s;
641 int err = 0;
643 BT_DBG("");
645 down_read(&bnep_session_sem);
647 s = __bnep_get_session(req->dst);
648 if (s) {
649 atomic_inc(&s->terminate);
650 wake_up_process(s->task);
651 } else
652 err = -ENOENT;
654 up_read(&bnep_session_sem);
655 return err;
658 static void __bnep_copy_ci(struct bnep_conninfo *ci, struct bnep_session *s)
660 memset(ci, 0, sizeof(*ci));
661 memcpy(ci->dst, s->eh.h_source, ETH_ALEN);
662 strcpy(ci->device, s->dev->name);
663 ci->flags = s->flags;
664 ci->state = s->state;
665 ci->role = s->role;
668 int bnep_get_connlist(struct bnep_connlist_req *req)
670 struct list_head *p;
671 int err = 0, n = 0;
673 down_read(&bnep_session_sem);
675 list_for_each(p, &bnep_session_list) {
676 struct bnep_session *s;
677 struct bnep_conninfo ci;
679 s = list_entry(p, struct bnep_session, list);
681 __bnep_copy_ci(&ci, s);
683 if (copy_to_user(req->ci, &ci, sizeof(ci))) {
684 err = -EFAULT;
685 break;
688 if (++n >= req->cnum)
689 break;
691 req->ci++;
693 req->cnum = n;
695 up_read(&bnep_session_sem);
696 return err;
699 int bnep_get_conninfo(struct bnep_conninfo *ci)
701 struct bnep_session *s;
702 int err = 0;
704 down_read(&bnep_session_sem);
706 s = __bnep_get_session(ci->dst);
707 if (s)
708 __bnep_copy_ci(ci, s);
709 else
710 err = -ENOENT;
712 up_read(&bnep_session_sem);
713 return err;
716 static int __init bnep_init(void)
718 char flt[50] = "";
720 #ifdef CONFIG_BT_BNEP_PROTO_FILTER
721 strcat(flt, "protocol ");
722 #endif
724 #ifdef CONFIG_BT_BNEP_MC_FILTER
725 strcat(flt, "multicast");
726 #endif
728 BT_INFO("BNEP (Ethernet Emulation) ver %s", VERSION);
729 if (flt[0])
730 BT_INFO("BNEP filters: %s", flt);
732 bnep_sock_init();
733 return 0;
736 static void __exit bnep_exit(void)
738 bnep_sock_cleanup();
741 module_init(bnep_init);
742 module_exit(bnep_exit);
744 module_param(compress_src, bool, 0644);
745 MODULE_PARM_DESC(compress_src, "Compress sources headers");
747 module_param(compress_dst, bool, 0644);
748 MODULE_PARM_DESC(compress_dst, "Compress destination headers");
750 MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
751 MODULE_DESCRIPTION("Bluetooth BNEP ver " VERSION);
752 MODULE_VERSION(VERSION);
753 MODULE_LICENSE("GPL");
754 MODULE_ALIAS("bt-proto-4");