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 /* 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
)
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
;
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
)
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
;
122 s
->proto_filter
[2].start
= ETH_P_IPX
;
123 s
->proto_filter
[2].end
= ETH_P_IPV6
;
127 static int bnep_ctrl_set_netfilter(struct bnep_session
*s
, __be16
*data
, int len
)
134 n
= get_unaligned_be16(data
);
141 BT_DBG("filter len %d", n
);
143 #ifdef CONFIG_BT_BNEP_PROTO_FILTER
145 if (n
<= BNEP_MAX_PROTO_FILTERS
) {
146 struct bnep_proto_filter
*f
= s
->proto_filter
;
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
));
161 bnep_set_default_proto_filter(s
);
163 bnep_send_rsp(s
, BNEP_FILTER_NET_TYPE_RSP
, BNEP_SUCCESS
);
165 bnep_send_rsp(s
, BNEP_FILTER_NET_TYPE_RSP
, BNEP_FILTER_LIMIT_REACHED
);
168 bnep_send_rsp(s
, BNEP_FILTER_NET_TYPE_RSP
, BNEP_FILTER_UNSUPPORTED_REQ
);
173 static int bnep_ctrl_set_mcfilter(struct bnep_session
*s
, u8
*data
, int len
)
180 n
= get_unaligned_be16(data
);
187 BT_DBG("filter len %d", n
);
189 #ifdef CONFIG_BT_BNEP_MC_FILTER
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 */
204 memcpy(a1
, 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) {
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
);
229 bnep_send_rsp(s
, BNEP_FILTER_MULTI_ADDR_RSP
, BNEP_FILTER_UNSUPPORTED_REQ
);
234 static int bnep_rx_control(struct bnep_session
*s
, void *data
, int len
)
236 u8 cmd
= *(u8
*)data
;
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 */
250 case BNEP_FILTER_NET_TYPE_SET
:
251 err
= bnep_ctrl_set_netfilter(s
, data
, len
);
254 case BNEP_FILTER_MULTI_ADDR_SET
:
255 err
= bnep_ctrl_set_mcfilter(s
, data
, len
);
258 case BNEP_SETUP_CONN_REQ
:
259 err
= bnep_send_rsp(s
, BNEP_SETUP_CONN_RSP
, BNEP_CONN_NOT_ALLOWED
);
264 pkt
[0] = BNEP_CONTROL
;
265 pkt
[1] = BNEP_CMD_NOT_UNDERSTOOD
;
267 bnep_send(s
, pkt
, sizeof(pkt
));
275 static int bnep_rx_extension(struct bnep_session
*s
, struct sk_buff
*skb
)
277 struct bnep_ext_hdr
*h
;
281 h
= (void *) skb
->data
;
282 if (!skb_pull(skb
, sizeof(*h
))) {
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
);
295 /* Unknown extension, skip it. */
299 if (!skb_pull(skb
, h
->len
)) {
303 } while (!err
&& (h
->type
& BNEP_EXT_HEADER
));
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
;
322 dev
->stats
.rx_bytes
+= skb
->len
;
324 type
= *(u8
*) skb
->data
;
327 if ((type
& BNEP_TYPE_MASK
) >= sizeof(__bnep_rx_hlen
))
330 if ((type
& BNEP_TYPE_MASK
) == BNEP_CONTROL
) {
331 bnep_rx_control(s
, skb
->data
, skb
->len
);
336 skb_reset_mac_header(skb
);
338 /* Verify and pull out header */
339 if (!skb_pull(skb
, __bnep_rx_hlen
[type
& BNEP_TYPE_MASK
]))
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)
349 /* Strip 802.1p header */
350 if (ntohs(s
->eh
.h_proto
) == 0x8100) {
351 if (!skb_pull(skb
, 4))
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
);
360 dev
->stats
.rx_dropped
++;
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
);
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));
378 case BNEP_COMPRESSED_DST_ONLY
:
379 memcpy(__skb_put(nskb
, ETH_ALEN
), skb_mac_header(skb
),
381 memcpy(__skb_put(nskb
, ETH_ALEN
+ 2), s
->eh
.h_source
,
386 memcpy(__skb_put(nskb
, ETH_ALEN
* 2), skb_mac_header(skb
),
388 put_unaligned(s
->eh
.h_proto
, (__be16
*) __skb_put(nskb
, 2));
392 skb_copy_from_linear_data(skb
, __skb_put(nskb
, skb
->len
), skb
->len
);
395 dev
->stats
.rx_packets
++;
396 nskb
->ip_summed
= CHECKSUM_NONE
;
397 nskb
->protocol
= eth_type_trans(nskb
, dev
);
402 dev
->stats
.rx_errors
++;
407 static u8 __bnep_tx_types
[] = {
409 BNEP_COMPRESSED_SRC_ONLY
,
410 BNEP_COMPRESSED_DST_ONLY
,
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
;
422 BT_DBG("skb %p dev %p type %d", skb
, skb
->dev
, skb
->pkt_type
);
425 /* Control frame sent by us */
429 iv
[il
++] = (struct kvec
) { &type
, 1 };
432 if (compress_src
&& !compare_ether_addr(eh
->h_dest
, s
->eh
.h_source
))
435 if (compress_dst
&& !compare_ether_addr(eh
->h_source
, s
->eh
.h_dest
))
439 skb_pull(skb
, ETH_ALEN
* 2);
441 type
= __bnep_tx_types
[type
];
443 case BNEP_COMPRESSED_SRC_ONLY
:
444 iv
[il
++] = (struct kvec
) { eh
->h_source
, ETH_ALEN
};
448 case BNEP_COMPRESSED_DST_ONLY
:
449 iv
[il
++] = (struct kvec
) { eh
->h_dest
, ETH_ALEN
};
455 iv
[il
++] = (struct kvec
) { skb
->data
, skb
->len
};
458 /* FIXME: linearize skb */
460 len
= kernel_sendmsg(sock
, &s
->msg
, iv
, il
, len
);
465 s
->dev
->stats
.tx_bytes
+= len
;
466 s
->dev
->stats
.tx_packets
++;
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
;
483 set_user_nice(current
, -15);
485 init_waitqueue_entry(&wait
, current
);
486 add_wait_queue(sk_sleep(sk
), &wait
);
488 set_current_state(TASK_INTERRUPTIBLE
);
490 if (atomic_read(&s
->terminate
))
493 while ((skb
= skb_dequeue(&sk
->sk_receive_queue
))) {
495 if (!skb_linearize(skb
))
496 bnep_rx_frame(s
, skb
);
501 if (sk
->sk_state
!= BT_CONNECTED
)
505 while ((skb
= skb_dequeue(&sk
->sk_write_queue
)))
506 if (bnep_tx_frame(s
, skb
))
508 netif_wake_queue(dev
);
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 */
529 __bnep_unlink_session(s
);
531 up_write(&bnep_session_sem
);
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
);
547 conn
= hci_conn_hash_lookup_ba(hdev
, ACL_LINK
, dst
);
551 return conn
? &conn
->dev
: NULL
;
554 static struct device_type bnep_type
= {
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
];
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",
577 down_write(&bnep_session_sem
);
579 ss
= __bnep_get_session(dst
);
580 if (ss
&& ss
->state
== BT_CONNECTED
) {
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
);
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
);
605 #ifdef CONFIG_BT_BNEP_PROTO_FILTER
606 /* Set default protocol filter */
607 bnep_set_default_proto_filter(s
);
610 SET_NETDEV_DEV(dev
, bnep_get_device(s
));
611 SET_NETDEV_DEVTYPE(dev
, &bnep_type
);
613 err
= register_netdev(dev
);
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
);
628 up_write(&bnep_session_sem
);
629 strcpy(req
->device
, dev
->name
);
633 up_write(&bnep_session_sem
);
638 int bnep_del_connection(struct bnep_conndel_req
*req
)
640 struct bnep_session
*s
;
645 down_read(&bnep_session_sem
);
647 s
= __bnep_get_session(req
->dst
);
649 atomic_inc(&s
->terminate
);
650 wake_up_process(s
->task
);
654 up_read(&bnep_session_sem
);
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
;
668 int bnep_get_connlist(struct bnep_connlist_req
*req
)
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
))) {
688 if (++n
>= req
->cnum
)
695 up_read(&bnep_session_sem
);
699 int bnep_get_conninfo(struct bnep_conninfo
*ci
)
701 struct bnep_session
*s
;
704 down_read(&bnep_session_sem
);
706 s
= __bnep_get_session(ci
->dst
);
708 __bnep_copy_ci(ci
, s
);
712 up_read(&bnep_session_sem
);
716 static int __init
bnep_init(void)
720 #ifdef CONFIG_BT_BNEP_PROTO_FILTER
721 strcat(flt
, "protocol ");
724 #ifdef CONFIG_BT_BNEP_MC_FILTER
725 strcat(flt
, "multicast");
728 BT_INFO("BNEP (Ethernet Emulation) ver %s", VERSION
);
730 BT_INFO("BNEP filters: %s", flt
);
736 static void __exit
bnep_exit(void)
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");