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>
29 #include <linux/kthread.h>
30 #include <linux/file.h>
31 #include <linux/etherdevice.h>
32 #include <asm/unaligned.h>
34 #include <net/bluetooth/bluetooth.h>
35 #include <net/bluetooth/hci_core.h>
36 #include <net/bluetooth/l2cap.h>
42 static bool compress_src = true;
43 static bool compress_dst = true;
45 static LIST_HEAD(bnep_session_list);
46 static DECLARE_RWSEM(bnep_session_sem);
48 static struct bnep_session *__bnep_get_session(u8 *dst)
50 struct bnep_session *s;
54 list_for_each_entry(s, &bnep_session_list, list)
55 if (ether_addr_equal(dst, s->eh.h_source))
61 static void __bnep_link_session(struct bnep_session *s)
63 list_add(&s->list, &bnep_session_list);
66 static void __bnep_unlink_session(struct bnep_session *s)
71 static int bnep_send(struct bnep_session *s, void *data, size_t len)
73 struct socket *sock = s->sock;
74 struct kvec iv = { data, len };
76 return kernel_sendmsg(sock, &s->msg, &iv, 1, len);
79 static int bnep_send_rsp(struct bnep_session *s, u8 ctrl, u16 resp)
81 struct bnep_control_rsp rsp;
82 rsp.type = BNEP_CONTROL;
84 rsp.resp = htons(resp);
85 return bnep_send(s, &rsp, sizeof(rsp));
88 #ifdef CONFIG_BT_BNEP_PROTO_FILTER
89 static inline void bnep_set_default_proto_filter(struct bnep_session *s)
92 s->proto_filter[0].start = ETH_P_IP;
93 s->proto_filter[0].end = ETH_P_ARP;
94 /* (RARP, AppleTalk) */
95 s->proto_filter[1].start = ETH_P_RARP;
96 s->proto_filter[1].end = ETH_P_AARP;
98 s->proto_filter[2].start = ETH_P_IPX;
99 s->proto_filter[2].end = ETH_P_IPV6;
103 static int bnep_ctrl_set_netfilter(struct bnep_session *s, __be16 *data, int len)
110 n = get_unaligned_be16(data);
117 BT_DBG("filter len %d", n);
119 #ifdef CONFIG_BT_BNEP_PROTO_FILTER
121 if (n <= BNEP_MAX_PROTO_FILTERS) {
122 struct bnep_proto_filter *f = s->proto_filter;
125 for (i = 0; i < n; i++) {
126 f[i].start = get_unaligned_be16(data++);
127 f[i].end = get_unaligned_be16(data++);
129 BT_DBG("proto filter start %d end %d",
130 f[i].start, f[i].end);
133 if (i < BNEP_MAX_PROTO_FILTERS)
134 memset(f + i, 0, sizeof(*f));
137 bnep_set_default_proto_filter(s);
139 bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_SUCCESS);
141 bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_FILTER_LIMIT_REACHED);
144 bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_FILTER_UNSUPPORTED_REQ);
149 static int bnep_ctrl_set_mcfilter(struct bnep_session *s, u8 *data, int len)
156 n = get_unaligned_be16(data);
163 BT_DBG("filter len %d", n);
165 #ifdef CONFIG_BT_BNEP_MC_FILTER
173 /* Always send broadcast */
174 set_bit(bnep_mc_hash(s->dev->broadcast), (ulong *) &s->mc_filter);
176 /* Add address ranges to the multicast hash */
180 memcpy(a1, data, ETH_ALEN);
185 BT_DBG("mc filter %s -> %s",
186 batostr((void *) a1), batostr((void *) a2));
188 /* Iterate from a1 to a2 */
189 set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter);
190 while (memcmp(a1, a2, 6) < 0 && s->mc_filter != ~0LL) {
193 while (i >= 0 && ++a1[i--] == 0)
196 set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter);
201 BT_DBG("mc filter hash 0x%llx", s->mc_filter);
203 bnep_send_rsp(s, BNEP_FILTER_MULTI_ADDR_RSP, BNEP_SUCCESS);
205 bnep_send_rsp(s, BNEP_FILTER_MULTI_ADDR_RSP, BNEP_FILTER_UNSUPPORTED_REQ);
210 static int bnep_rx_control(struct bnep_session *s, void *data, int len)
212 u8 cmd = *(u8 *)data;
219 case BNEP_CMD_NOT_UNDERSTOOD:
220 case BNEP_SETUP_CONN_RSP:
221 case BNEP_FILTER_NET_TYPE_RSP:
222 case BNEP_FILTER_MULTI_ADDR_RSP:
223 /* Ignore these for now */
226 case BNEP_FILTER_NET_TYPE_SET:
227 err = bnep_ctrl_set_netfilter(s, data, len);
230 case BNEP_FILTER_MULTI_ADDR_SET:
231 err = bnep_ctrl_set_mcfilter(s, data, len);
234 case BNEP_SETUP_CONN_REQ:
235 err = bnep_send_rsp(s, BNEP_SETUP_CONN_RSP, BNEP_CONN_NOT_ALLOWED);
240 pkt[0] = BNEP_CONTROL;
241 pkt[1] = BNEP_CMD_NOT_UNDERSTOOD;
243 bnep_send(s, pkt, sizeof(pkt));
251 static int bnep_rx_extension(struct bnep_session *s, struct sk_buff *skb)
253 struct bnep_ext_hdr *h;
257 h = (void *) skb->data;
258 if (!skb_pull(skb, sizeof(*h))) {
263 BT_DBG("type 0x%x len %d", h->type, h->len);
265 switch (h->type & BNEP_TYPE_MASK) {
266 case BNEP_EXT_CONTROL:
267 bnep_rx_control(s, skb->data, skb->len);
271 /* Unknown extension, skip it. */
275 if (!skb_pull(skb, h->len)) {
279 } while (!err && (h->type & BNEP_EXT_HEADER));
284 static u8 __bnep_rx_hlen[] = {
285 ETH_HLEN, /* BNEP_GENERAL */
286 0, /* BNEP_CONTROL */
287 2, /* BNEP_COMPRESSED */
288 ETH_ALEN + 2, /* BNEP_COMPRESSED_SRC_ONLY */
289 ETH_ALEN + 2 /* BNEP_COMPRESSED_DST_ONLY */
292 static int bnep_rx_frame(struct bnep_session *s, struct sk_buff *skb)
294 struct net_device *dev = s->dev;
295 struct sk_buff *nskb;
298 dev->stats.rx_bytes += skb->len;
300 type = *(u8 *) skb->data;
303 if ((type & BNEP_TYPE_MASK) >= sizeof(__bnep_rx_hlen))
306 if ((type & BNEP_TYPE_MASK) == BNEP_CONTROL) {
307 bnep_rx_control(s, skb->data, skb->len);
312 skb_reset_mac_header(skb);
314 /* Verify and pull out header */
315 if (!skb_pull(skb, __bnep_rx_hlen[type & BNEP_TYPE_MASK]))
318 s->eh.h_proto = get_unaligned((__be16 *) (skb->data - 2));
320 if (type & BNEP_EXT_HEADER) {
321 if (bnep_rx_extension(s, skb) < 0)
325 /* Strip 802.1p header */
326 if (ntohs(s->eh.h_proto) == ETH_P_8021Q) {
327 if (!skb_pull(skb, 4))
329 s->eh.h_proto = get_unaligned((__be16 *) (skb->data - 2));
332 /* We have to alloc new skb and copy data here :(. Because original skb
333 * may not be modified and because of the alignment requirements. */
334 nskb = alloc_skb(2 + ETH_HLEN + skb->len, GFP_KERNEL);
336 dev->stats.rx_dropped++;
340 skb_reserve(nskb, 2);
342 /* Decompress header and construct ether frame */
343 switch (type & BNEP_TYPE_MASK) {
344 case BNEP_COMPRESSED:
345 memcpy(__skb_put(nskb, ETH_HLEN), &s->eh, ETH_HLEN);
348 case BNEP_COMPRESSED_SRC_ONLY:
349 memcpy(__skb_put(nskb, ETH_ALEN), s->eh.h_dest, ETH_ALEN);
350 memcpy(__skb_put(nskb, ETH_ALEN), skb_mac_header(skb), ETH_ALEN);
351 put_unaligned(s->eh.h_proto, (__be16 *) __skb_put(nskb, 2));
354 case BNEP_COMPRESSED_DST_ONLY:
355 memcpy(__skb_put(nskb, ETH_ALEN), skb_mac_header(skb),
357 memcpy(__skb_put(nskb, ETH_ALEN + 2), s->eh.h_source,
362 memcpy(__skb_put(nskb, ETH_ALEN * 2), skb_mac_header(skb),
364 put_unaligned(s->eh.h_proto, (__be16 *) __skb_put(nskb, 2));
368 skb_copy_from_linear_data(skb, __skb_put(nskb, skb->len), skb->len);
371 dev->stats.rx_packets++;
372 nskb->ip_summed = CHECKSUM_NONE;
373 nskb->protocol = eth_type_trans(nskb, dev);
378 dev->stats.rx_errors++;
383 static u8 __bnep_tx_types[] = {
385 BNEP_COMPRESSED_SRC_ONLY,
386 BNEP_COMPRESSED_DST_ONLY,
390 static int bnep_tx_frame(struct bnep_session *s, struct sk_buff *skb)
392 struct ethhdr *eh = (void *) skb->data;
393 struct socket *sock = s->sock;
398 BT_DBG("skb %p dev %p type %d", skb, skb->dev, skb->pkt_type);
401 /* Control frame sent by us */
405 iv[il++] = (struct kvec) { &type, 1 };
408 if (compress_src && ether_addr_equal(eh->h_dest, s->eh.h_source))
411 if (compress_dst && ether_addr_equal(eh->h_source, s->eh.h_dest))
415 skb_pull(skb, ETH_ALEN * 2);
417 type = __bnep_tx_types[type];
419 case BNEP_COMPRESSED_SRC_ONLY:
420 iv[il++] = (struct kvec) { eh->h_source, ETH_ALEN };
424 case BNEP_COMPRESSED_DST_ONLY:
425 iv[il++] = (struct kvec) { eh->h_dest, ETH_ALEN };
431 iv[il++] = (struct kvec) { skb->data, skb->len };
434 /* FIXME: linearize skb */
436 len = kernel_sendmsg(sock, &s->msg, iv, il, len);
441 s->dev->stats.tx_bytes += len;
442 s->dev->stats.tx_packets++;
449 static int bnep_session(void *arg)
451 struct bnep_session *s = arg;
452 struct net_device *dev = s->dev;
453 struct sock *sk = s->sock->sk;
459 set_user_nice(current, -15);
461 init_waitqueue_entry(&wait, current);
462 add_wait_queue(sk_sleep(sk), &wait);
464 set_current_state(TASK_INTERRUPTIBLE);
466 if (atomic_read(&s->terminate))
469 while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
471 if (!skb_linearize(skb))
472 bnep_rx_frame(s, skb);
477 if (sk->sk_state != BT_CONNECTED)
481 while ((skb = skb_dequeue(&sk->sk_write_queue)))
482 if (bnep_tx_frame(s, skb))
484 netif_wake_queue(dev);
488 __set_current_state(TASK_RUNNING);
489 remove_wait_queue(sk_sleep(sk), &wait);
491 /* Cleanup session */
492 down_write(&bnep_session_sem);
494 /* Delete network device */
495 unregister_netdev(dev);
497 /* Wakeup user-space polling for socket errors */
498 s->sock->sk->sk_err = EUNATCH;
500 wake_up_interruptible(sk_sleep(s->sock->sk));
502 /* Release the socket */
505 __bnep_unlink_session(s);
507 up_write(&bnep_session_sem);
509 module_put_and_exit(0);
513 static struct device *bnep_get_device(struct bnep_session *session)
515 bdaddr_t *src = &bt_sk(session->sock->sk)->src;
516 bdaddr_t *dst = &bt_sk(session->sock->sk)->dst;
517 struct hci_dev *hdev;
518 struct hci_conn *conn;
520 hdev = hci_get_route(dst, src);
524 conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, dst);
528 return conn ? &conn->dev : NULL;
531 static struct device_type bnep_type = {
535 int bnep_add_connection(struct bnep_connadd_req *req, struct socket *sock)
537 struct net_device *dev;
538 struct bnep_session *s, *ss;
539 u8 dst[ETH_ALEN], src[ETH_ALEN];
544 baswap((void *) dst, &bt_sk(sock->sk)->dst);
545 baswap((void *) src, &bt_sk(sock->sk)->src);
547 /* session struct allocated as private part of net_device */
548 dev = alloc_netdev(sizeof(struct bnep_session),
549 (*req->device) ? req->device : "bnep%d",
554 down_write(&bnep_session_sem);
556 ss = __bnep_get_session(dst);
557 if (ss && ss->state == BT_CONNECTED) {
562 s = netdev_priv(dev);
564 /* This is rx header therefore addresses are swapped.
565 * ie. eh.h_dest is our local address. */
566 memcpy(s->eh.h_dest, &src, ETH_ALEN);
567 memcpy(s->eh.h_source, &dst, ETH_ALEN);
568 memcpy(dev->dev_addr, s->eh.h_dest, ETH_ALEN);
573 s->state = BT_CONNECTED;
575 s->msg.msg_flags = MSG_NOSIGNAL;
577 #ifdef CONFIG_BT_BNEP_MC_FILTER
578 /* Set default mc filter */
579 set_bit(bnep_mc_hash(dev->broadcast), (ulong *) &s->mc_filter);
582 #ifdef CONFIG_BT_BNEP_PROTO_FILTER
583 /* Set default protocol filter */
584 bnep_set_default_proto_filter(s);
587 SET_NETDEV_DEV(dev, bnep_get_device(s));
588 SET_NETDEV_DEVTYPE(dev, &bnep_type);
590 err = register_netdev(dev);
594 __bnep_link_session(s);
596 __module_get(THIS_MODULE);
597 s->task = kthread_run(bnep_session, s, "kbnepd %s", dev->name);
598 if (IS_ERR(s->task)) {
599 /* Session thread start failed, gotta cleanup. */
600 module_put(THIS_MODULE);
601 unregister_netdev(dev);
602 __bnep_unlink_session(s);
603 err = PTR_ERR(s->task);
607 up_write(&bnep_session_sem);
608 strcpy(req->device, dev->name);
612 up_write(&bnep_session_sem);
617 int bnep_del_connection(struct bnep_conndel_req *req)
619 struct bnep_session *s;
624 down_read(&bnep_session_sem);
626 s = __bnep_get_session(req->dst);
628 atomic_inc(&s->terminate);
629 wake_up_process(s->task);
633 up_read(&bnep_session_sem);
637 static void __bnep_copy_ci(struct bnep_conninfo *ci, struct bnep_session *s)
639 memset(ci, 0, sizeof(*ci));
640 memcpy(ci->dst, s->eh.h_source, ETH_ALEN);
641 strcpy(ci->device, s->dev->name);
642 ci->flags = s->flags;
643 ci->state = s->state;
647 int bnep_get_connlist(struct bnep_connlist_req *req)
649 struct bnep_session *s;
652 down_read(&bnep_session_sem);
654 list_for_each_entry(s, &bnep_session_list, list) {
655 struct bnep_conninfo ci;
657 __bnep_copy_ci(&ci, s);
659 if (copy_to_user(req->ci, &ci, sizeof(ci))) {
664 if (++n >= req->cnum)
671 up_read(&bnep_session_sem);
675 int bnep_get_conninfo(struct bnep_conninfo *ci)
677 struct bnep_session *s;
680 down_read(&bnep_session_sem);
682 s = __bnep_get_session(ci->dst);
684 __bnep_copy_ci(ci, s);
688 up_read(&bnep_session_sem);
692 static int __init bnep_init(void)
696 #ifdef CONFIG_BT_BNEP_PROTO_FILTER
697 strcat(flt, "protocol ");
700 #ifdef CONFIG_BT_BNEP_MC_FILTER
701 strcat(flt, "multicast");
704 BT_INFO("BNEP (Ethernet Emulation) ver %s", VERSION);
706 BT_INFO("BNEP filters: %s", flt);
712 static void __exit bnep_exit(void)
717 module_init(bnep_init);
718 module_exit(bnep_exit);
720 module_param(compress_src, bool, 0644);
721 MODULE_PARM_DESC(compress_src, "Compress sources headers");
723 module_param(compress_dst, bool, 0644);
724 MODULE_PARM_DESC(compress_dst, "Compress destination headers");
726 MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
727 MODULE_DESCRIPTION("Bluetooth BNEP ver " VERSION);
728 MODULE_VERSION(VERSION);
729 MODULE_LICENSE("GPL");
730 MODULE_ALIAS("bt-proto-4");