2 BNEP implementation for Linux Bluetooth stack (BlueZ).
3 Copyright (C) 2001-2002 Inventel Systemes
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>
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>
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;
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))
80 static void __bnep_link_session(struct bnep_session *s)
82 list_add(&s->list, &bnep_session_list);
85 static void __bnep_unlink_session(struct bnep_session *s)
90 static int bnep_send(struct bnep_session *s, void *data, size_t len)
92 struct socket *sock = s->sock;
93 struct kvec iv = { data, len };
95 return kernel_sendmsg(sock, &s->msg, &iv, 1, len);
98 static int bnep_send_rsp(struct bnep_session *s, u8 ctrl, u16 resp)
100 struct bnep_control_rsp rsp;
101 rsp.type = BNEP_CONTROL;
103 rsp.resp = htons(resp);
104 return bnep_send(s, &rsp, sizeof(rsp));
107 #ifdef CONFIG_BT_BNEP_PROTO_FILTER
108 static inline void bnep_set_default_proto_filter(struct bnep_session *s)
111 s->proto_filter[0].start = ETH_P_IP;
112 s->proto_filter[0].end = ETH_P_ARP;
113 /* (RARP, AppleTalk) */
114 s->proto_filter[1].start = ETH_P_RARP;
115 s->proto_filter[1].end = ETH_P_AARP;
117 s->proto_filter[2].start = ETH_P_IPX;
118 s->proto_filter[2].end = ETH_P_IPV6;
122 static int bnep_ctrl_set_netfilter(struct bnep_session *s, __be16 *data, int len)
129 n = get_unaligned_be16(data);
136 BT_DBG("filter len %d", n);
138 #ifdef CONFIG_BT_BNEP_PROTO_FILTER
140 if (n <= BNEP_MAX_PROTO_FILTERS) {
141 struct bnep_proto_filter *f = s->proto_filter;
144 for (i = 0; i < n; i++) {
145 f[i].start = get_unaligned_be16(data++);
146 f[i].end = get_unaligned_be16(data++);
148 BT_DBG("proto filter start %d end %d",
149 f[i].start, f[i].end);
152 if (i < BNEP_MAX_PROTO_FILTERS)
153 memset(f + i, 0, sizeof(*f));
156 bnep_set_default_proto_filter(s);
158 bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_SUCCESS);
160 bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_FILTER_LIMIT_REACHED);
163 bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_FILTER_UNSUPPORTED_REQ);
168 static int bnep_ctrl_set_mcfilter(struct bnep_session *s, u8 *data, int len)
175 n = get_unaligned_be16(data);
182 BT_DBG("filter len %d", n);
184 #ifdef CONFIG_BT_BNEP_MC_FILTER
192 /* Always send broadcast */
193 set_bit(bnep_mc_hash(s->dev->broadcast), (ulong *) &s->mc_filter);
195 /* Add address ranges to the multicast hash */
199 memcpy(a1, data, ETH_ALEN);
204 BT_DBG("mc filter %s -> %s",
205 batostr((void *) a1), batostr((void *) a2));
207 /* Iterate from a1 to a2 */
208 set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter);
209 while (memcmp(a1, a2, 6) < 0 && s->mc_filter != ~0LL) {
212 while (i >= 0 && ++a1[i--] == 0)
215 set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter);
220 BT_DBG("mc filter hash 0x%llx", s->mc_filter);
222 bnep_send_rsp(s, BNEP_FILTER_MULTI_ADDR_RSP, BNEP_SUCCESS);
224 bnep_send_rsp(s, BNEP_FILTER_MULTI_ADDR_RSP, BNEP_FILTER_UNSUPPORTED_REQ);
229 static int bnep_rx_control(struct bnep_session *s, void *data, int len)
231 u8 cmd = *(u8 *)data;
238 case BNEP_CMD_NOT_UNDERSTOOD:
239 case BNEP_SETUP_CONN_RSP:
240 case BNEP_FILTER_NET_TYPE_RSP:
241 case BNEP_FILTER_MULTI_ADDR_RSP:
242 /* Ignore these for now */
245 case BNEP_FILTER_NET_TYPE_SET:
246 err = bnep_ctrl_set_netfilter(s, data, len);
249 case BNEP_FILTER_MULTI_ADDR_SET:
250 err = bnep_ctrl_set_mcfilter(s, data, len);
253 case BNEP_SETUP_CONN_REQ:
254 err = bnep_send_rsp(s, BNEP_SETUP_CONN_RSP, BNEP_CONN_NOT_ALLOWED);
259 pkt[0] = BNEP_CONTROL;
260 pkt[1] = BNEP_CMD_NOT_UNDERSTOOD;
262 bnep_send(s, pkt, sizeof(pkt));
270 static int bnep_rx_extension(struct bnep_session *s, struct sk_buff *skb)
272 struct bnep_ext_hdr *h;
276 h = (void *) skb->data;
277 if (!skb_pull(skb, sizeof(*h))) {
282 BT_DBG("type 0x%x len %d", h->type, h->len);
284 switch (h->type & BNEP_TYPE_MASK) {
285 case BNEP_EXT_CONTROL:
286 bnep_rx_control(s, skb->data, skb->len);
290 /* Unknown extension, skip it. */
294 if (!skb_pull(skb, h->len)) {
298 } while (!err && (h->type & BNEP_EXT_HEADER));
303 static u8 __bnep_rx_hlen[] = {
304 ETH_HLEN, /* BNEP_GENERAL */
305 0, /* BNEP_CONTROL */
306 2, /* BNEP_COMPRESSED */
307 ETH_ALEN + 2, /* BNEP_COMPRESSED_SRC_ONLY */
308 ETH_ALEN + 2 /* BNEP_COMPRESSED_DST_ONLY */
311 static inline int bnep_rx_frame(struct bnep_session *s, struct sk_buff *skb)
313 struct net_device *dev = s->dev;
314 struct sk_buff *nskb;
317 dev->stats.rx_bytes += skb->len;
319 type = *(u8 *) skb->data;
322 if ((type & BNEP_TYPE_MASK) >= sizeof(__bnep_rx_hlen))
325 if ((type & BNEP_TYPE_MASK) == BNEP_CONTROL) {
326 bnep_rx_control(s, skb->data, skb->len);
331 skb_reset_mac_header(skb);
333 /* Verify and pull out header */
334 if (!skb_pull(skb, __bnep_rx_hlen[type & BNEP_TYPE_MASK]))
337 s->eh.h_proto = get_unaligned((__be16 *) (skb->data - 2));
339 if (type & BNEP_EXT_HEADER) {
340 if (bnep_rx_extension(s, skb) < 0)
344 /* Strip 802.1p header */
345 if (ntohs(s->eh.h_proto) == 0x8100) {
346 if (!skb_pull(skb, 4))
348 s->eh.h_proto = get_unaligned((__be16 *) (skb->data - 2));
351 /* We have to alloc new skb and copy data here :(. Because original skb
352 * may not be modified and because of the alignment requirements. */
353 nskb = alloc_skb(2 + ETH_HLEN + skb->len, GFP_KERNEL);
355 dev->stats.rx_dropped++;
359 skb_reserve(nskb, 2);
361 /* Decompress header and construct ether frame */
362 switch (type & BNEP_TYPE_MASK) {
363 case BNEP_COMPRESSED:
364 memcpy(__skb_put(nskb, ETH_HLEN), &s->eh, ETH_HLEN);
367 case BNEP_COMPRESSED_SRC_ONLY:
368 memcpy(__skb_put(nskb, ETH_ALEN), s->eh.h_dest, ETH_ALEN);
369 memcpy(__skb_put(nskb, ETH_ALEN), skb_mac_header(skb), ETH_ALEN);
370 put_unaligned(s->eh.h_proto, (__be16 *) __skb_put(nskb, 2));
373 case BNEP_COMPRESSED_DST_ONLY:
374 memcpy(__skb_put(nskb, ETH_ALEN), skb_mac_header(skb),
376 memcpy(__skb_put(nskb, ETH_ALEN + 2), s->eh.h_source,
381 memcpy(__skb_put(nskb, ETH_ALEN * 2), skb_mac_header(skb),
383 put_unaligned(s->eh.h_proto, (__be16 *) __skb_put(nskb, 2));
387 skb_copy_from_linear_data(skb, __skb_put(nskb, skb->len), skb->len);
390 dev->stats.rx_packets++;
391 nskb->ip_summed = CHECKSUM_NONE;
392 nskb->protocol = eth_type_trans(nskb, dev);
397 dev->stats.rx_errors++;
402 static u8 __bnep_tx_types[] = {
404 BNEP_COMPRESSED_SRC_ONLY,
405 BNEP_COMPRESSED_DST_ONLY,
409 static inline int bnep_tx_frame(struct bnep_session *s, struct sk_buff *skb)
411 struct ethhdr *eh = (void *) skb->data;
412 struct socket *sock = s->sock;
417 BT_DBG("skb %p dev %p type %d", skb, skb->dev, skb->pkt_type);
420 /* Control frame sent by us */
424 iv[il++] = (struct kvec) { &type, 1 };
427 if (compress_src && !compare_ether_addr(eh->h_dest, s->eh.h_source))
430 if (compress_dst && !compare_ether_addr(eh->h_source, s->eh.h_dest))
434 skb_pull(skb, ETH_ALEN * 2);
436 type = __bnep_tx_types[type];
438 case BNEP_COMPRESSED_SRC_ONLY:
439 iv[il++] = (struct kvec) { eh->h_source, ETH_ALEN };
443 case BNEP_COMPRESSED_DST_ONLY:
444 iv[il++] = (struct kvec) { eh->h_dest, ETH_ALEN };
450 iv[il++] = (struct kvec) { skb->data, skb->len };
453 /* FIXME: linearize skb */
455 len = kernel_sendmsg(sock, &s->msg, iv, il, len);
460 s->dev->stats.tx_bytes += len;
461 s->dev->stats.tx_packets++;
468 static int bnep_session(void *arg)
470 struct bnep_session *s = arg;
471 struct net_device *dev = s->dev;
472 struct sock *sk = s->sock->sk;
478 set_user_nice(current, -15);
480 init_waitqueue_entry(&wait, current);
481 add_wait_queue(sk_sleep(sk), &wait);
483 set_current_state(TASK_INTERRUPTIBLE);
485 if (atomic_read(&s->terminate))
488 while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
490 if (!skb_linearize(skb))
491 bnep_rx_frame(s, skb);
496 if (sk->sk_state != BT_CONNECTED)
500 while ((skb = skb_dequeue(&sk->sk_write_queue)))
501 if (bnep_tx_frame(s, skb))
503 netif_wake_queue(dev);
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 */
524 __bnep_unlink_session(s);
526 up_write(&bnep_session_sem);
528 module_put_and_exit(0);
532 static struct device *bnep_get_device(struct bnep_session *session)
534 bdaddr_t *src = &bt_sk(session->sock->sk)->src;
535 bdaddr_t *dst = &bt_sk(session->sock->sk)->dst;
536 struct hci_dev *hdev;
537 struct hci_conn *conn;
539 hdev = hci_get_route(dst, src);
543 conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, dst);
547 return conn ? &conn->dev : NULL;
550 static struct device_type bnep_type = {
554 int bnep_add_connection(struct bnep_connadd_req *req, struct socket *sock)
556 struct net_device *dev;
557 struct bnep_session *s, *ss;
558 u8 dst[ETH_ALEN], src[ETH_ALEN];
563 baswap((void *) dst, &bt_sk(sock->sk)->dst);
564 baswap((void *) src, &bt_sk(sock->sk)->src);
566 /* session struct allocated as private part of net_device */
567 dev = alloc_netdev(sizeof(struct bnep_session),
568 (*req->device) ? req->device : "bnep%d",
573 down_write(&bnep_session_sem);
575 ss = __bnep_get_session(dst);
576 if (ss && ss->state == BT_CONNECTED) {
581 s = netdev_priv(dev);
583 /* This is rx header therefore addresses are swapped.
584 * ie. eh.h_dest is our local address. */
585 memcpy(s->eh.h_dest, &src, ETH_ALEN);
586 memcpy(s->eh.h_source, &dst, ETH_ALEN);
587 memcpy(dev->dev_addr, s->eh.h_dest, ETH_ALEN);
592 s->state = BT_CONNECTED;
594 s->msg.msg_flags = MSG_NOSIGNAL;
596 #ifdef CONFIG_BT_BNEP_MC_FILTER
597 /* Set default mc filter */
598 set_bit(bnep_mc_hash(dev->broadcast), (ulong *) &s->mc_filter);
601 #ifdef CONFIG_BT_BNEP_PROTO_FILTER
602 /* Set default protocol filter */
603 bnep_set_default_proto_filter(s);
606 SET_NETDEV_DEV(dev, bnep_get_device(s));
607 SET_NETDEV_DEVTYPE(dev, &bnep_type);
609 err = register_netdev(dev);
613 __bnep_link_session(s);
615 __module_get(THIS_MODULE);
616 s->task = kthread_run(bnep_session, s, "kbnepd %s", dev->name);
617 if (IS_ERR(s->task)) {
618 /* Session thread start failed, gotta cleanup. */
619 module_put(THIS_MODULE);
620 unregister_netdev(dev);
621 __bnep_unlink_session(s);
622 err = PTR_ERR(s->task);
626 up_write(&bnep_session_sem);
627 strcpy(req->device, dev->name);
631 up_write(&bnep_session_sem);
636 int bnep_del_connection(struct bnep_conndel_req *req)
638 struct bnep_session *s;
643 down_read(&bnep_session_sem);
645 s = __bnep_get_session(req->dst);
647 atomic_inc(&s->terminate);
648 wake_up_process(s->task);
652 up_read(&bnep_session_sem);
656 static void __bnep_copy_ci(struct bnep_conninfo *ci, struct bnep_session *s)
658 memset(ci, 0, sizeof(*ci));
659 memcpy(ci->dst, s->eh.h_source, ETH_ALEN);
660 strcpy(ci->device, s->dev->name);
661 ci->flags = s->flags;
662 ci->state = s->state;
666 int bnep_get_connlist(struct bnep_connlist_req *req)
671 down_read(&bnep_session_sem);
673 list_for_each(p, &bnep_session_list) {
674 struct bnep_session *s;
675 struct bnep_conninfo ci;
677 s = list_entry(p, struct bnep_session, list);
679 __bnep_copy_ci(&ci, s);
681 if (copy_to_user(req->ci, &ci, sizeof(ci))) {
686 if (++n >= req->cnum)
693 up_read(&bnep_session_sem);
697 int bnep_get_conninfo(struct bnep_conninfo *ci)
699 struct bnep_session *s;
702 down_read(&bnep_session_sem);
704 s = __bnep_get_session(ci->dst);
706 __bnep_copy_ci(ci, s);
710 up_read(&bnep_session_sem);
714 static int __init bnep_init(void)
718 #ifdef CONFIG_BT_BNEP_PROTO_FILTER
719 strcat(flt, "protocol ");
722 #ifdef CONFIG_BT_BNEP_MC_FILTER
723 strcat(flt, "multicast");
726 BT_INFO("BNEP (Ethernet Emulation) ver %s", VERSION);
728 BT_INFO("BNEP filters: %s", flt);
734 static void __exit bnep_exit(void)
739 module_init(bnep_init);
740 module_exit(bnep_exit);
742 module_param(compress_src, bool, 0644);
743 MODULE_PARM_DESC(compress_src, "Compress sources headers");
745 module_param(compress_dst, bool, 0644);
746 MODULE_PARM_DESC(compress_dst, "Compress destination headers");
748 MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
749 MODULE_DESCRIPTION("Bluetooth BNEP ver " VERSION);
750 MODULE_VERSION(VERSION);
751 MODULE_LICENSE("GPL");
752 MODULE_ALIAS("bt-proto-4");