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.
29 * $Id: core.c,v 1.20 2002/08/04 21:23:58 maxk Exp $
32 #include <linux/config.h>
33 #include <linux/module.h>
35 #include <linux/kernel.h>
36 #include <linux/sched.h>
37 #include <linux/signal.h>
38 #include <linux/init.h>
39 #include <linux/wait.h>
40 #include <linux/errno.h>
41 #include <linux/smp_lock.h>
42 #include <linux/net.h>
45 #include <linux/socket.h>
46 #include <linux/file.h>
48 #include <linux/netdevice.h>
49 #include <linux/etherdevice.h>
50 #include <linux/skbuff.h>
52 #include <asm/unaligned.h>
54 #include <net/bluetooth/bluetooth.h>
55 #include <net/bluetooth/l2cap.h>
59 #ifndef CONFIG_BT_BNEP_DEBUG
66 static LIST_HEAD(bnep_session_list);
67 static DECLARE_RWSEM(bnep_session_sem);
69 static struct bnep_session *__bnep_get_session(u8 *dst)
71 struct bnep_session *s;
76 list_for_each(p, &bnep_session_list) {
77 s = list_entry(p, struct bnep_session, list);
78 if (!memcmp(dst, s->eh.h_source, ETH_ALEN))
84 static void __bnep_link_session(struct bnep_session *s)
86 /* It's safe to call __module_get() here because sessions are added
87 by the socket layer which has to hold the refference to this module.
89 __module_get(THIS_MODULE);
90 list_add(&s->list, &bnep_session_list);
93 static void __bnep_unlink_session(struct bnep_session *s)
96 module_put(THIS_MODULE);
99 static int bnep_send(struct bnep_session *s, void *data, size_t len)
101 struct socket *sock = s->sock;
102 struct iovec iv = { data, len };
104 s->msg.msg_iov = &iv;
105 s->msg.msg_iovlen = 1;
106 return sock_sendmsg(sock, &s->msg, len);
109 static int bnep_send_rsp(struct bnep_session *s, u8 ctrl, u16 resp)
111 struct bnep_control_rsp rsp;
112 rsp.type = BNEP_CONTROL;
114 rsp.resp = htons(resp);
115 return bnep_send(s, &rsp, sizeof(rsp));
118 static int bnep_ctrl_set_netfilter(struct bnep_session *s, u16 *data, int len)
125 n = ntohs(get_unaligned(data));
131 BT_DBG("filter len %d", n);
133 #ifdef CONFIG_BT_BNEP_PROTO_FILTER
135 if (n <= BNEP_MAX_PROTO_FILTERS) {
136 struct bnep_proto_filter *f = s->proto_filter;
139 for (i = 0; i < n; i++) {
140 f[i].start = get_unaligned(data++);
141 f[i].end = get_unaligned(data++);
143 BT_DBG("proto filter start %d end %d",
144 f[i].start, f[i].end);
146 if (i < BNEP_MAX_PROTO_FILTERS)
147 memset(f + i, 0, sizeof(*f));
149 bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_SUCCESS);
151 bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_FILTER_LIMIT_REACHED);
154 bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_FILTER_UNSUPPORTED_REQ);
159 static int bnep_ctrl_set_mcfilter(struct bnep_session *s, u8 *data, int len)
166 n = ntohs(get_unaligned((u16 *) data));
172 BT_DBG("filter len %d", n);
174 #ifdef CONFIG_BT_BNEP_MC_FILTER
180 /* Always send broadcast */
181 set_bit(bnep_mc_hash(s->dev->broadcast), (ulong *) &s->mc_filter);
183 /* Add address ranges to the multicast hash */
187 memcpy(a1, data, ETH_ALEN); data += ETH_ALEN;
188 a2 = data; data += ETH_ALEN;
190 BT_DBG("mc filter %s -> %s",
191 batostr((void *) a1), batostr((void *) a2));
193 #define INCA(a) { int i = 5; while (i >=0 && ++a[i--] == 0); }
195 /* Iterate from a1 to a2 */
196 set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter);
197 while (memcmp(a1, a2, 6) < 0 && s->mc_filter != ~0LL) {
199 set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter);
204 BT_DBG("mc filter hash 0x%llx", s->mc_filter);
206 bnep_send_rsp(s, BNEP_FILTER_MULTI_ADDR_RSP, BNEP_SUCCESS);
208 bnep_send_rsp(s, BNEP_FILTER_MULTI_ADDR_RSP, BNEP_FILTER_UNSUPPORTED_REQ);
213 static int bnep_rx_control(struct bnep_session *s, void *data, int len)
215 u8 cmd = *(u8 *)data;
221 case BNEP_CMD_NOT_UNDERSTOOD:
222 case BNEP_SETUP_CONN_REQ:
223 case BNEP_SETUP_CONN_RSP:
224 case BNEP_FILTER_NET_TYPE_RSP:
225 case BNEP_FILTER_MULTI_ADDR_RSP:
226 /* Ignore these for now */
229 case BNEP_FILTER_NET_TYPE_SET:
230 err = bnep_ctrl_set_netfilter(s, data, len);
233 case BNEP_FILTER_MULTI_ADDR_SET:
234 err = bnep_ctrl_set_mcfilter(s, data, len);
239 pkt[0] = BNEP_CONTROL;
240 pkt[1] = BNEP_CMD_NOT_UNDERSTOOD;
242 bnep_send(s, pkt, sizeof(pkt));
250 static int bnep_rx_extension(struct bnep_session *s, struct sk_buff *skb)
252 struct bnep_ext_hdr *h;
256 h = (void *) skb->data;
257 if (!skb_pull(skb, sizeof(*h))) {
262 BT_DBG("type 0x%x len %d", h->type, h->len);
264 switch (h->type & BNEP_TYPE_MASK) {
265 case BNEP_EXT_CONTROL:
266 bnep_rx_control(s, skb->data, skb->len);
270 /* Unknown extension, skip it. */
274 if (!skb_pull(skb, h->len)) {
278 } while (!err && (h->type & BNEP_EXT_HEADER));
283 static u8 __bnep_rx_hlen[] = {
284 ETH_HLEN, /* BNEP_GENERAL */
285 0, /* BNEP_CONTROL */
286 2, /* BNEP_COMPRESSED */
287 ETH_ALEN + 2, /* BNEP_COMPRESSED_SRC_ONLY */
288 ETH_ALEN + 2 /* BNEP_COMPRESSED_DST_ONLY */
290 #define BNEP_RX_TYPES (sizeof(__bnep_rx_hlen) - 1)
292 static inline 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->last_rx = jiffies;
299 s->stats.rx_bytes += skb->len;
301 type = *(u8 *) skb->data; skb_pull(skb, 1);
303 if ((type & BNEP_TYPE_MASK) > BNEP_RX_TYPES)
306 if ((type & BNEP_TYPE_MASK) == BNEP_CONTROL) {
307 bnep_rx_control(s, skb->data, skb->len);
312 skb->mac.raw = skb->data;
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((u16 *) (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) == 0x8100) {
327 if (!skb_pull(skb, 4))
329 s->eh.h_proto = get_unaligned((u16 *) (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 s->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.raw, ETH_ALEN);
351 put_unaligned(s->eh.h_proto, (u16 *) __skb_put(nskb, 2));
354 case BNEP_COMPRESSED_DST_ONLY:
355 memcpy(__skb_put(nskb, ETH_ALEN), skb->mac.raw, ETH_ALEN);
356 memcpy(__skb_put(nskb, ETH_ALEN + 2), s->eh.h_source, ETH_ALEN + 2);
360 memcpy(__skb_put(nskb, ETH_ALEN * 2), skb->mac.raw, ETH_ALEN * 2);
361 put_unaligned(s->eh.h_proto, (u16 *) __skb_put(nskb, 2));
365 memcpy(__skb_put(nskb, skb->len), skb->data, skb->len);
368 s->stats.rx_packets++;
370 nskb->ip_summed = CHECKSUM_UNNECESSARY;
371 nskb->protocol = eth_type_trans(nskb, dev);
376 s->stats.rx_errors++;
381 static u8 __bnep_tx_types[] = {
383 BNEP_COMPRESSED_SRC_ONLY,
384 BNEP_COMPRESSED_DST_ONLY,
388 static inline int bnep_tx_frame(struct bnep_session *s, struct sk_buff *skb)
390 struct ethhdr *eh = (void *) skb->data;
391 struct socket *sock = s->sock;
396 BT_DBG("skb %p dev %p type %d", skb, skb->dev, skb->pkt_type);
399 /* Control frame sent by us */
403 iv[il++] = (struct iovec) { &type, 1 };
406 if (!memcmp(eh->h_dest, s->eh.h_source, ETH_ALEN))
409 if (!memcmp(eh->h_source, s->eh.h_dest, ETH_ALEN))
413 skb_pull(skb, ETH_ALEN * 2);
415 type = __bnep_tx_types[type];
417 case BNEP_COMPRESSED_SRC_ONLY:
418 iv[il++] = (struct iovec) { eh->h_source, ETH_ALEN };
422 case BNEP_COMPRESSED_DST_ONLY:
423 iv[il++] = (struct iovec) { eh->h_dest, ETH_ALEN };
429 iv[il++] = (struct iovec) { skb->data, skb->len };
432 /* FIXME: linearize skb */
435 s->msg.msg_iovlen = il;
436 len = sock_sendmsg(sock, &s->msg, len);
441 s->stats.tx_bytes += len;
442 s->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 daemonize("kbnepd %s", dev->name);
460 set_user_nice(current, -15);
461 current->flags |= PF_NOFREEZE;
465 init_waitqueue_entry(&wait, current);
466 add_wait_queue(sk->sk_sleep, &wait);
467 while (!atomic_read(&s->killed)) {
468 set_current_state(TASK_INTERRUPTIBLE);
471 while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
473 bnep_rx_frame(s, skb);
476 if (sk->sk_state != BT_CONNECTED)
480 while ((skb = skb_dequeue(&sk->sk_write_queue)))
481 if (bnep_tx_frame(s, skb))
483 netif_wake_queue(dev);
487 set_current_state(TASK_RUNNING);
488 remove_wait_queue(sk->sk_sleep, &wait);
490 /* Cleanup session */
491 down_write(&bnep_session_sem);
493 /* Delete network device */
494 unregister_netdev(dev);
496 /* Release the socket */
499 __bnep_unlink_session(s);
501 up_write(&bnep_session_sem);
506 int bnep_add_connection(struct bnep_connadd_req *req, struct socket *sock)
508 struct net_device *dev;
509 struct bnep_session *s, *ss;
510 u8 dst[ETH_ALEN], src[ETH_ALEN];
515 baswap((void *) dst, &bt_sk(sock->sk)->dst);
516 baswap((void *) src, &bt_sk(sock->sk)->src);
518 /* session struct allocated as private part of net_device */
519 dev = alloc_netdev(sizeof(struct bnep_session),
520 (*req->device) ? req->device : "bnep%d",
526 down_write(&bnep_session_sem);
528 ss = __bnep_get_session(dst);
529 if (ss && ss->state == BT_CONNECTED) {
536 /* This is rx header therefore addresses are swapped.
537 * ie eh.h_dest is our local address. */
538 memcpy(s->eh.h_dest, &src, ETH_ALEN);
539 memcpy(s->eh.h_source, &dst, ETH_ALEN);
540 memcpy(dev->dev_addr, s->eh.h_dest, ETH_ALEN);
545 s->state = BT_CONNECTED;
547 s->msg.msg_flags = MSG_NOSIGNAL;
549 #ifdef CONFIG_BT_BNEP_MC_FILTER
550 /* Set default mc filter */
551 set_bit(bnep_mc_hash(dev->broadcast), (ulong *) &s->mc_filter);
554 #ifdef CONFIG_BT_BNEP_PROTO_FILTER
555 /* Set default protocol filter */
558 s->proto_filter[0].start = htons(0x0800);
559 s->proto_filter[0].end = htons(0x0806);
560 /* (RARP, AppleTalk) */
561 s->proto_filter[1].start = htons(0x8035);
562 s->proto_filter[1].end = htons(0x80F3);
564 s->proto_filter[2].start = htons(0x8137);
565 s->proto_filter[2].end = htons(0x86DD);
568 err = register_netdev(dev);
573 __bnep_link_session(s);
575 err = kernel_thread(bnep_session, s, CLONE_KERNEL);
577 /* Session thread start failed, gotta cleanup. */
578 unregister_netdev(dev);
579 __bnep_unlink_session(s);
583 up_write(&bnep_session_sem);
584 strcpy(req->device, dev->name);
588 up_write(&bnep_session_sem);
593 int bnep_del_connection(struct bnep_conndel_req *req)
595 struct bnep_session *s;
600 down_read(&bnep_session_sem);
602 s = __bnep_get_session(req->dst);
604 /* Wakeup user-space which is polling for socket errors.
605 * This is temporary hack untill we have shutdown in L2CAP */
606 s->sock->sk->sk_err = EUNATCH;
608 /* Kill session thread */
609 atomic_inc(&s->killed);
610 wake_up_interruptible(s->sock->sk->sk_sleep);
614 up_read(&bnep_session_sem);
618 static void __bnep_copy_ci(struct bnep_conninfo *ci, struct bnep_session *s)
620 memcpy(ci->dst, s->eh.h_source, ETH_ALEN);
621 strcpy(ci->device, s->dev->name);
622 ci->flags = s->flags;
623 ci->state = s->state;
627 int bnep_get_connlist(struct bnep_connlist_req *req)
632 down_read(&bnep_session_sem);
634 list_for_each(p, &bnep_session_list) {
635 struct bnep_session *s;
636 struct bnep_conninfo ci;
638 s = list_entry(p, struct bnep_session, list);
640 __bnep_copy_ci(&ci, s);
642 if (copy_to_user(req->ci, &ci, sizeof(ci))) {
647 if (++n >= req->cnum)
654 up_read(&bnep_session_sem);
658 int bnep_get_conninfo(struct bnep_conninfo *ci)
660 struct bnep_session *s;
663 down_read(&bnep_session_sem);
665 s = __bnep_get_session(ci->dst);
667 __bnep_copy_ci(ci, s);
671 up_read(&bnep_session_sem);
675 static int __init bnep_init(void)
681 #ifdef CONFIG_BT_BNEP_PROTO_FILTER
682 strcat(flt, "protocol ");
685 #ifdef CONFIG_BT_BNEP_MC_FILTER
686 strcat(flt, "multicast");
689 BT_INFO("BNEP (Ethernet Emulation) ver %s", VERSION);
691 BT_INFO("BNEP filters: %s", flt);
697 static void __exit bnep_exit(void)
702 module_init(bnep_init);
703 module_exit(bnep_exit);
705 MODULE_AUTHOR("David Libault <david.libault@inventel.fr>, Maxim Krasnyansky <maxk@qualcomm.com>");
706 MODULE_DESCRIPTION("Bluetooth BNEP ver " VERSION);
707 MODULE_VERSION(VERSION);
708 MODULE_LICENSE("GPL");
709 MODULE_ALIAS("bt-proto-4");