Loading...
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>
7
8 Copyright (C) 2002 Maxim Krasnyansky <maxk@qualcomm.com>
9
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;
13
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.
22
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.
26*/
27
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>
33
34#include <net/bluetooth/bluetooth.h>
35#include <net/bluetooth/l2cap.h>
36#include <net/bluetooth/hci_core.h>
37
38#include "bnep.h"
39
40#define VERSION "1.3"
41
42static bool compress_src = true;
43static bool compress_dst = true;
44
45static LIST_HEAD(bnep_session_list);
46static DECLARE_RWSEM(bnep_session_sem);
47
48static struct bnep_session *__bnep_get_session(u8 *dst)
49{
50 struct bnep_session *s;
51
52 BT_DBG("");
53
54 list_for_each_entry(s, &bnep_session_list, list)
55 if (ether_addr_equal(dst, s->eh.h_source))
56 return s;
57
58 return NULL;
59}
60
61static void __bnep_link_session(struct bnep_session *s)
62{
63 list_add(&s->list, &bnep_session_list);
64}
65
66static void __bnep_unlink_session(struct bnep_session *s)
67{
68 list_del(&s->list);
69}
70
71static int bnep_send(struct bnep_session *s, void *data, size_t len)
72{
73 struct socket *sock = s->sock;
74 struct kvec iv = { data, len };
75
76 return kernel_sendmsg(sock, &s->msg, &iv, 1, len);
77}
78
79static int bnep_send_rsp(struct bnep_session *s, u8 ctrl, u16 resp)
80{
81 struct bnep_control_rsp rsp;
82 rsp.type = BNEP_CONTROL;
83 rsp.ctrl = ctrl;
84 rsp.resp = htons(resp);
85 return bnep_send(s, &rsp, sizeof(rsp));
86}
87
88#ifdef CONFIG_BT_BNEP_PROTO_FILTER
89static inline void bnep_set_default_proto_filter(struct bnep_session *s)
90{
91 /* (IPv4, ARP) */
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;
97 /* (IPX, IPv6) */
98 s->proto_filter[2].start = ETH_P_IPX;
99 s->proto_filter[2].end = ETH_P_IPV6;
100}
101#endif
102
103static int bnep_ctrl_set_netfilter(struct bnep_session *s, __be16 *data, int len)
104{
105 int n;
106
107 if (len < 2)
108 return -EILSEQ;
109
110 n = get_unaligned_be16(data);
111 data++;
112 len -= 2;
113
114 if (len < n)
115 return -EILSEQ;
116
117 BT_DBG("filter len %d", n);
118
119#ifdef CONFIG_BT_BNEP_PROTO_FILTER
120 n /= 4;
121 if (n <= BNEP_MAX_PROTO_FILTERS) {
122 struct bnep_proto_filter *f = s->proto_filter;
123 int i;
124
125 for (i = 0; i < n; i++) {
126 f[i].start = get_unaligned_be16(data++);
127 f[i].end = get_unaligned_be16(data++);
128
129 BT_DBG("proto filter start %d end %d",
130 f[i].start, f[i].end);
131 }
132
133 if (i < BNEP_MAX_PROTO_FILTERS)
134 memset(f + i, 0, sizeof(*f));
135
136 if (n == 0)
137 bnep_set_default_proto_filter(s);
138
139 bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_SUCCESS);
140 } else {
141 bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_FILTER_LIMIT_REACHED);
142 }
143#else
144 bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_FILTER_UNSUPPORTED_REQ);
145#endif
146 return 0;
147}
148
149static int bnep_ctrl_set_mcfilter(struct bnep_session *s, u8 *data, int len)
150{
151 int n;
152
153 if (len < 2)
154 return -EILSEQ;
155
156 n = get_unaligned_be16(data);
157 data += 2;
158 len -= 2;
159
160 if (len < n)
161 return -EILSEQ;
162
163 BT_DBG("filter len %d", n);
164
165#ifdef CONFIG_BT_BNEP_MC_FILTER
166 n /= (ETH_ALEN * 2);
167
168 if (n > 0) {
169 int i;
170
171 s->mc_filter = 0;
172
173 /* Always send broadcast */
174 set_bit(bnep_mc_hash(s->dev->broadcast), (ulong *) &s->mc_filter);
175
176 /* Add address ranges to the multicast hash */
177 for (; n > 0; n--) {
178 u8 a1[6], *a2;
179
180 memcpy(a1, data, ETH_ALEN);
181 data += ETH_ALEN;
182 a2 = data;
183 data += ETH_ALEN;
184
185 BT_DBG("mc filter %pMR -> %pMR", a1, a2);
186
187 /* Iterate from a1 to a2 */
188 set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter);
189 while (memcmp(a1, a2, 6) < 0 && s->mc_filter != ~0LL) {
190 /* Increment a1 */
191 i = 5;
192 while (i >= 0 && ++a1[i--] == 0)
193 ;
194
195 set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter);
196 }
197 }
198 }
199
200 BT_DBG("mc filter hash 0x%llx", s->mc_filter);
201
202 bnep_send_rsp(s, BNEP_FILTER_MULTI_ADDR_RSP, BNEP_SUCCESS);
203#else
204 bnep_send_rsp(s, BNEP_FILTER_MULTI_ADDR_RSP, BNEP_FILTER_UNSUPPORTED_REQ);
205#endif
206 return 0;
207}
208
209static int bnep_rx_control(struct bnep_session *s, void *data, int len)
210{
211 u8 cmd = *(u8 *)data;
212 int err = 0;
213
214 data++;
215 len--;
216
217 switch (cmd) {
218 case BNEP_CMD_NOT_UNDERSTOOD:
219 case BNEP_SETUP_CONN_RSP:
220 case BNEP_FILTER_NET_TYPE_RSP:
221 case BNEP_FILTER_MULTI_ADDR_RSP:
222 /* Ignore these for now */
223 break;
224
225 case BNEP_FILTER_NET_TYPE_SET:
226 err = bnep_ctrl_set_netfilter(s, data, len);
227 break;
228
229 case BNEP_FILTER_MULTI_ADDR_SET:
230 err = bnep_ctrl_set_mcfilter(s, data, len);
231 break;
232
233 case BNEP_SETUP_CONN_REQ:
234 err = bnep_send_rsp(s, BNEP_SETUP_CONN_RSP, BNEP_CONN_NOT_ALLOWED);
235 break;
236
237 default: {
238 u8 pkt[3];
239 pkt[0] = BNEP_CONTROL;
240 pkt[1] = BNEP_CMD_NOT_UNDERSTOOD;
241 pkt[2] = cmd;
242 bnep_send(s, pkt, sizeof(pkt));
243 }
244 break;
245 }
246
247 return err;
248}
249
250static int bnep_rx_extension(struct bnep_session *s, struct sk_buff *skb)
251{
252 struct bnep_ext_hdr *h;
253 int err = 0;
254
255 do {
256 h = (void *) skb->data;
257 if (!skb_pull(skb, sizeof(*h))) {
258 err = -EILSEQ;
259 break;
260 }
261
262 BT_DBG("type 0x%x len %d", h->type, h->len);
263
264 switch (h->type & BNEP_TYPE_MASK) {
265 case BNEP_EXT_CONTROL:
266 bnep_rx_control(s, skb->data, skb->len);
267 break;
268
269 default:
270 /* Unknown extension, skip it. */
271 break;
272 }
273
274 if (!skb_pull(skb, h->len)) {
275 err = -EILSEQ;
276 break;
277 }
278 } while (!err && (h->type & BNEP_EXT_HEADER));
279
280 return err;
281}
282
283static 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 */
289};
290
291static int bnep_rx_frame(struct bnep_session *s, struct sk_buff *skb)
292{
293 struct net_device *dev = s->dev;
294 struct sk_buff *nskb;
295 u8 type;
296
297 dev->stats.rx_bytes += skb->len;
298
299 type = *(u8 *) skb->data;
300 skb_pull(skb, 1);
301
302 if ((type & BNEP_TYPE_MASK) >= sizeof(__bnep_rx_hlen))
303 goto badframe;
304
305 if ((type & BNEP_TYPE_MASK) == BNEP_CONTROL) {
306 bnep_rx_control(s, skb->data, skb->len);
307 kfree_skb(skb);
308 return 0;
309 }
310
311 skb_reset_mac_header(skb);
312
313 /* Verify and pull out header */
314 if (!skb_pull(skb, __bnep_rx_hlen[type & BNEP_TYPE_MASK]))
315 goto badframe;
316
317 s->eh.h_proto = get_unaligned((__be16 *) (skb->data - 2));
318
319 if (type & BNEP_EXT_HEADER) {
320 if (bnep_rx_extension(s, skb) < 0)
321 goto badframe;
322 }
323
324 /* Strip 802.1p header */
325 if (ntohs(s->eh.h_proto) == ETH_P_8021Q) {
326 if (!skb_pull(skb, 4))
327 goto badframe;
328 s->eh.h_proto = get_unaligned((__be16 *) (skb->data - 2));
329 }
330
331 /* We have to alloc new skb and copy data here :(. Because original skb
332 * may not be modified and because of the alignment requirements. */
333 nskb = alloc_skb(2 + ETH_HLEN + skb->len, GFP_KERNEL);
334 if (!nskb) {
335 dev->stats.rx_dropped++;
336 kfree_skb(skb);
337 return -ENOMEM;
338 }
339 skb_reserve(nskb, 2);
340
341 /* Decompress header and construct ether frame */
342 switch (type & BNEP_TYPE_MASK) {
343 case BNEP_COMPRESSED:
344 memcpy(__skb_put(nskb, ETH_HLEN), &s->eh, ETH_HLEN);
345 break;
346
347 case BNEP_COMPRESSED_SRC_ONLY:
348 memcpy(__skb_put(nskb, ETH_ALEN), s->eh.h_dest, ETH_ALEN);
349 memcpy(__skb_put(nskb, ETH_ALEN), skb_mac_header(skb), ETH_ALEN);
350 put_unaligned(s->eh.h_proto, (__be16 *) __skb_put(nskb, 2));
351 break;
352
353 case BNEP_COMPRESSED_DST_ONLY:
354 memcpy(__skb_put(nskb, ETH_ALEN), skb_mac_header(skb),
355 ETH_ALEN);
356 memcpy(__skb_put(nskb, ETH_ALEN + 2), s->eh.h_source,
357 ETH_ALEN + 2);
358 break;
359
360 case BNEP_GENERAL:
361 memcpy(__skb_put(nskb, ETH_ALEN * 2), skb_mac_header(skb),
362 ETH_ALEN * 2);
363 put_unaligned(s->eh.h_proto, (__be16 *) __skb_put(nskb, 2));
364 break;
365 }
366
367 skb_copy_from_linear_data(skb, __skb_put(nskb, skb->len), skb->len);
368 kfree_skb(skb);
369
370 dev->stats.rx_packets++;
371 nskb->ip_summed = CHECKSUM_NONE;
372 nskb->protocol = eth_type_trans(nskb, dev);
373 netif_rx_ni(nskb);
374 return 0;
375
376badframe:
377 dev->stats.rx_errors++;
378 kfree_skb(skb);
379 return 0;
380}
381
382static u8 __bnep_tx_types[] = {
383 BNEP_GENERAL,
384 BNEP_COMPRESSED_SRC_ONLY,
385 BNEP_COMPRESSED_DST_ONLY,
386 BNEP_COMPRESSED
387};
388
389static int bnep_tx_frame(struct bnep_session *s, struct sk_buff *skb)
390{
391 struct ethhdr *eh = (void *) skb->data;
392 struct socket *sock = s->sock;
393 struct kvec iv[3];
394 int len = 0, il = 0;
395 u8 type = 0;
396
397 BT_DBG("skb %p dev %p type %d", skb, skb->dev, skb->pkt_type);
398
399 if (!skb->dev) {
400 /* Control frame sent by us */
401 goto send;
402 }
403
404 iv[il++] = (struct kvec) { &type, 1 };
405 len++;
406
407 if (compress_src && ether_addr_equal(eh->h_dest, s->eh.h_source))
408 type |= 0x01;
409
410 if (compress_dst && ether_addr_equal(eh->h_source, s->eh.h_dest))
411 type |= 0x02;
412
413 if (type)
414 skb_pull(skb, ETH_ALEN * 2);
415
416 type = __bnep_tx_types[type];
417 switch (type) {
418 case BNEP_COMPRESSED_SRC_ONLY:
419 iv[il++] = (struct kvec) { eh->h_source, ETH_ALEN };
420 len += ETH_ALEN;
421 break;
422
423 case BNEP_COMPRESSED_DST_ONLY:
424 iv[il++] = (struct kvec) { eh->h_dest, ETH_ALEN };
425 len += ETH_ALEN;
426 break;
427 }
428
429send:
430 iv[il++] = (struct kvec) { skb->data, skb->len };
431 len += skb->len;
432
433 /* FIXME: linearize skb */
434 {
435 len = kernel_sendmsg(sock, &s->msg, iv, il, len);
436 }
437 kfree_skb(skb);
438
439 if (len > 0) {
440 s->dev->stats.tx_bytes += len;
441 s->dev->stats.tx_packets++;
442 return 0;
443 }
444
445 return len;
446}
447
448static int bnep_session(void *arg)
449{
450 struct bnep_session *s = arg;
451 struct net_device *dev = s->dev;
452 struct sock *sk = s->sock->sk;
453 struct sk_buff *skb;
454 wait_queue_t wait;
455
456 BT_DBG("");
457
458 set_user_nice(current, -15);
459
460 init_waitqueue_entry(&wait, current);
461 add_wait_queue(sk_sleep(sk), &wait);
462 while (1) {
463 set_current_state(TASK_INTERRUPTIBLE);
464
465 if (atomic_read(&s->terminate))
466 break;
467 /* RX */
468 while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
469 skb_orphan(skb);
470 if (!skb_linearize(skb))
471 bnep_rx_frame(s, skb);
472 else
473 kfree_skb(skb);
474 }
475
476 if (sk->sk_state != BT_CONNECTED)
477 break;
478
479 /* TX */
480 while ((skb = skb_dequeue(&sk->sk_write_queue)))
481 if (bnep_tx_frame(s, skb))
482 break;
483 netif_wake_queue(dev);
484
485 schedule();
486 }
487 __set_current_state(TASK_RUNNING);
488 remove_wait_queue(sk_sleep(sk), &wait);
489
490 /* Cleanup session */
491 down_write(&bnep_session_sem);
492
493 /* Delete network device */
494 unregister_netdev(dev);
495
496 /* Wakeup user-space polling for socket errors */
497 s->sock->sk->sk_err = EUNATCH;
498
499 wake_up_interruptible(sk_sleep(s->sock->sk));
500
501 /* Release the socket */
502 fput(s->sock->file);
503
504 __bnep_unlink_session(s);
505
506 up_write(&bnep_session_sem);
507 free_netdev(dev);
508 module_put_and_exit(0);
509 return 0;
510}
511
512static struct device *bnep_get_device(struct bnep_session *session)
513{
514 struct hci_conn *conn;
515
516 conn = l2cap_pi(session->sock->sk)->chan->conn->hcon;
517 if (!conn)
518 return NULL;
519
520 return &conn->dev;
521}
522
523static struct device_type bnep_type = {
524 .name = "bluetooth",
525};
526
527int bnep_add_connection(struct bnep_connadd_req *req, struct socket *sock)
528{
529 struct net_device *dev;
530 struct bnep_session *s, *ss;
531 u8 dst[ETH_ALEN], src[ETH_ALEN];
532 int err;
533
534 BT_DBG("");
535
536 baswap((void *) dst, &l2cap_pi(sock->sk)->chan->dst);
537 baswap((void *) src, &l2cap_pi(sock->sk)->chan->src);
538
539 /* session struct allocated as private part of net_device */
540 dev = alloc_netdev(sizeof(struct bnep_session),
541 (*req->device) ? req->device : "bnep%d",
542 bnep_net_setup);
543 if (!dev)
544 return -ENOMEM;
545
546 down_write(&bnep_session_sem);
547
548 ss = __bnep_get_session(dst);
549 if (ss && ss->state == BT_CONNECTED) {
550 err = -EEXIST;
551 goto failed;
552 }
553
554 s = netdev_priv(dev);
555
556 /* This is rx header therefore addresses are swapped.
557 * ie. eh.h_dest is our local address. */
558 memcpy(s->eh.h_dest, &src, ETH_ALEN);
559 memcpy(s->eh.h_source, &dst, ETH_ALEN);
560 memcpy(dev->dev_addr, s->eh.h_dest, ETH_ALEN);
561
562 s->dev = dev;
563 s->sock = sock;
564 s->role = req->role;
565 s->state = BT_CONNECTED;
566
567 s->msg.msg_flags = MSG_NOSIGNAL;
568
569#ifdef CONFIG_BT_BNEP_MC_FILTER
570 /* Set default mc filter */
571 set_bit(bnep_mc_hash(dev->broadcast), (ulong *) &s->mc_filter);
572#endif
573
574#ifdef CONFIG_BT_BNEP_PROTO_FILTER
575 /* Set default protocol filter */
576 bnep_set_default_proto_filter(s);
577#endif
578
579 SET_NETDEV_DEV(dev, bnep_get_device(s));
580 SET_NETDEV_DEVTYPE(dev, &bnep_type);
581
582 err = register_netdev(dev);
583 if (err)
584 goto failed;
585
586 __bnep_link_session(s);
587
588 __module_get(THIS_MODULE);
589 s->task = kthread_run(bnep_session, s, "kbnepd %s", dev->name);
590 if (IS_ERR(s->task)) {
591 /* Session thread start failed, gotta cleanup. */
592 module_put(THIS_MODULE);
593 unregister_netdev(dev);
594 __bnep_unlink_session(s);
595 err = PTR_ERR(s->task);
596 goto failed;
597 }
598
599 up_write(&bnep_session_sem);
600 strcpy(req->device, dev->name);
601 return 0;
602
603failed:
604 up_write(&bnep_session_sem);
605 free_netdev(dev);
606 return err;
607}
608
609int bnep_del_connection(struct bnep_conndel_req *req)
610{
611 struct bnep_session *s;
612 int err = 0;
613
614 BT_DBG("");
615
616 down_read(&bnep_session_sem);
617
618 s = __bnep_get_session(req->dst);
619 if (s) {
620 atomic_inc(&s->terminate);
621 wake_up_process(s->task);
622 } else
623 err = -ENOENT;
624
625 up_read(&bnep_session_sem);
626 return err;
627}
628
629static void __bnep_copy_ci(struct bnep_conninfo *ci, struct bnep_session *s)
630{
631 memset(ci, 0, sizeof(*ci));
632 memcpy(ci->dst, s->eh.h_source, ETH_ALEN);
633 strcpy(ci->device, s->dev->name);
634 ci->flags = s->flags;
635 ci->state = s->state;
636 ci->role = s->role;
637}
638
639int bnep_get_connlist(struct bnep_connlist_req *req)
640{
641 struct bnep_session *s;
642 int err = 0, n = 0;
643
644 down_read(&bnep_session_sem);
645
646 list_for_each_entry(s, &bnep_session_list, list) {
647 struct bnep_conninfo ci;
648
649 __bnep_copy_ci(&ci, s);
650
651 if (copy_to_user(req->ci, &ci, sizeof(ci))) {
652 err = -EFAULT;
653 break;
654 }
655
656 if (++n >= req->cnum)
657 break;
658
659 req->ci++;
660 }
661 req->cnum = n;
662
663 up_read(&bnep_session_sem);
664 return err;
665}
666
667int bnep_get_conninfo(struct bnep_conninfo *ci)
668{
669 struct bnep_session *s;
670 int err = 0;
671
672 down_read(&bnep_session_sem);
673
674 s = __bnep_get_session(ci->dst);
675 if (s)
676 __bnep_copy_ci(ci, s);
677 else
678 err = -ENOENT;
679
680 up_read(&bnep_session_sem);
681 return err;
682}
683
684static int __init bnep_init(void)
685{
686 char flt[50] = "";
687
688#ifdef CONFIG_BT_BNEP_PROTO_FILTER
689 strcat(flt, "protocol ");
690#endif
691
692#ifdef CONFIG_BT_BNEP_MC_FILTER
693 strcat(flt, "multicast");
694#endif
695
696 BT_INFO("BNEP (Ethernet Emulation) ver %s", VERSION);
697 if (flt[0])
698 BT_INFO("BNEP filters: %s", flt);
699
700 bnep_sock_init();
701 return 0;
702}
703
704static void __exit bnep_exit(void)
705{
706 bnep_sock_cleanup();
707}
708
709module_init(bnep_init);
710module_exit(bnep_exit);
711
712module_param(compress_src, bool, 0644);
713MODULE_PARM_DESC(compress_src, "Compress sources headers");
714
715module_param(compress_dst, bool, 0644);
716MODULE_PARM_DESC(compress_dst, "Compress destination headers");
717
718MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
719MODULE_DESCRIPTION("Bluetooth BNEP ver " VERSION);
720MODULE_VERSION(VERSION);
721MODULE_LICENSE("GPL");
722MODULE_ALIAS("bt-proto-4");
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>
7
8 Copyright (C) 2002 Maxim Krasnyansky <maxk@qualcomm.com>
9
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;
13
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.
22
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.
26*/
27
28#include <linux/module.h>
29
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>
41
42#include <linux/socket.h>
43#include <linux/file.h>
44
45#include <linux/netdevice.h>
46#include <linux/etherdevice.h>
47#include <linux/skbuff.h>
48
49#include <asm/unaligned.h>
50
51#include <net/bluetooth/bluetooth.h>
52#include <net/bluetooth/hci_core.h>
53#include <net/bluetooth/l2cap.h>
54
55#include "bnep.h"
56
57#define VERSION "1.3"
58
59static bool compress_src = true;
60static bool compress_dst = true;
61
62static LIST_HEAD(bnep_session_list);
63static DECLARE_RWSEM(bnep_session_sem);
64
65static struct bnep_session *__bnep_get_session(u8 *dst)
66{
67 struct bnep_session *s;
68
69 BT_DBG("");
70
71 list_for_each_entry(s, &bnep_session_list, list)
72 if (ether_addr_equal(dst, s->eh.h_source))
73 return s;
74
75 return NULL;
76}
77
78static void __bnep_link_session(struct bnep_session *s)
79{
80 list_add(&s->list, &bnep_session_list);
81}
82
83static void __bnep_unlink_session(struct bnep_session *s)
84{
85 list_del(&s->list);
86}
87
88static int bnep_send(struct bnep_session *s, void *data, size_t len)
89{
90 struct socket *sock = s->sock;
91 struct kvec iv = { data, len };
92
93 return kernel_sendmsg(sock, &s->msg, &iv, 1, len);
94}
95
96static int bnep_send_rsp(struct bnep_session *s, u8 ctrl, u16 resp)
97{
98 struct bnep_control_rsp rsp;
99 rsp.type = BNEP_CONTROL;
100 rsp.ctrl = ctrl;
101 rsp.resp = htons(resp);
102 return bnep_send(s, &rsp, sizeof(rsp));
103}
104
105#ifdef CONFIG_BT_BNEP_PROTO_FILTER
106static inline void bnep_set_default_proto_filter(struct bnep_session *s)
107{
108 /* (IPv4, ARP) */
109 s->proto_filter[0].start = ETH_P_IP;
110 s->proto_filter[0].end = ETH_P_ARP;
111 /* (RARP, AppleTalk) */
112 s->proto_filter[1].start = ETH_P_RARP;
113 s->proto_filter[1].end = ETH_P_AARP;
114 /* (IPX, IPv6) */
115 s->proto_filter[2].start = ETH_P_IPX;
116 s->proto_filter[2].end = ETH_P_IPV6;
117}
118#endif
119
120static int bnep_ctrl_set_netfilter(struct bnep_session *s, __be16 *data, int len)
121{
122 int n;
123
124 if (len < 2)
125 return -EILSEQ;
126
127 n = get_unaligned_be16(data);
128 data++;
129 len -= 2;
130
131 if (len < n)
132 return -EILSEQ;
133
134 BT_DBG("filter len %d", n);
135
136#ifdef CONFIG_BT_BNEP_PROTO_FILTER
137 n /= 4;
138 if (n <= BNEP_MAX_PROTO_FILTERS) {
139 struct bnep_proto_filter *f = s->proto_filter;
140 int i;
141
142 for (i = 0; i < n; i++) {
143 f[i].start = get_unaligned_be16(data++);
144 f[i].end = get_unaligned_be16(data++);
145
146 BT_DBG("proto filter start %d end %d",
147 f[i].start, f[i].end);
148 }
149
150 if (i < BNEP_MAX_PROTO_FILTERS)
151 memset(f + i, 0, sizeof(*f));
152
153 if (n == 0)
154 bnep_set_default_proto_filter(s);
155
156 bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_SUCCESS);
157 } else {
158 bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_FILTER_LIMIT_REACHED);
159 }
160#else
161 bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_FILTER_UNSUPPORTED_REQ);
162#endif
163 return 0;
164}
165
166static int bnep_ctrl_set_mcfilter(struct bnep_session *s, u8 *data, int len)
167{
168 int n;
169
170 if (len < 2)
171 return -EILSEQ;
172
173 n = get_unaligned_be16(data);
174 data += 2;
175 len -= 2;
176
177 if (len < n)
178 return -EILSEQ;
179
180 BT_DBG("filter len %d", n);
181
182#ifdef CONFIG_BT_BNEP_MC_FILTER
183 n /= (ETH_ALEN * 2);
184
185 if (n > 0) {
186 int i;
187
188 s->mc_filter = 0;
189
190 /* Always send broadcast */
191 set_bit(bnep_mc_hash(s->dev->broadcast), (ulong *) &s->mc_filter);
192
193 /* Add address ranges to the multicast hash */
194 for (; n > 0; n--) {
195 u8 a1[6], *a2;
196
197 memcpy(a1, data, ETH_ALEN);
198 data += ETH_ALEN;
199 a2 = data;
200 data += ETH_ALEN;
201
202 BT_DBG("mc filter %s -> %s",
203 batostr((void *) a1), batostr((void *) a2));
204
205 /* Iterate from a1 to a2 */
206 set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter);
207 while (memcmp(a1, a2, 6) < 0 && s->mc_filter != ~0LL) {
208 /* Increment a1 */
209 i = 5;
210 while (i >= 0 && ++a1[i--] == 0)
211 ;
212
213 set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter);
214 }
215 }
216 }
217
218 BT_DBG("mc filter hash 0x%llx", s->mc_filter);
219
220 bnep_send_rsp(s, BNEP_FILTER_MULTI_ADDR_RSP, BNEP_SUCCESS);
221#else
222 bnep_send_rsp(s, BNEP_FILTER_MULTI_ADDR_RSP, BNEP_FILTER_UNSUPPORTED_REQ);
223#endif
224 return 0;
225}
226
227static int bnep_rx_control(struct bnep_session *s, void *data, int len)
228{
229 u8 cmd = *(u8 *)data;
230 int err = 0;
231
232 data++;
233 len--;
234
235 switch (cmd) {
236 case BNEP_CMD_NOT_UNDERSTOOD:
237 case BNEP_SETUP_CONN_RSP:
238 case BNEP_FILTER_NET_TYPE_RSP:
239 case BNEP_FILTER_MULTI_ADDR_RSP:
240 /* Ignore these for now */
241 break;
242
243 case BNEP_FILTER_NET_TYPE_SET:
244 err = bnep_ctrl_set_netfilter(s, data, len);
245 break;
246
247 case BNEP_FILTER_MULTI_ADDR_SET:
248 err = bnep_ctrl_set_mcfilter(s, data, len);
249 break;
250
251 case BNEP_SETUP_CONN_REQ:
252 err = bnep_send_rsp(s, BNEP_SETUP_CONN_RSP, BNEP_CONN_NOT_ALLOWED);
253 break;
254
255 default: {
256 u8 pkt[3];
257 pkt[0] = BNEP_CONTROL;
258 pkt[1] = BNEP_CMD_NOT_UNDERSTOOD;
259 pkt[2] = cmd;
260 bnep_send(s, pkt, sizeof(pkt));
261 }
262 break;
263 }
264
265 return err;
266}
267
268static int bnep_rx_extension(struct bnep_session *s, struct sk_buff *skb)
269{
270 struct bnep_ext_hdr *h;
271 int err = 0;
272
273 do {
274 h = (void *) skb->data;
275 if (!skb_pull(skb, sizeof(*h))) {
276 err = -EILSEQ;
277 break;
278 }
279
280 BT_DBG("type 0x%x len %d", h->type, h->len);
281
282 switch (h->type & BNEP_TYPE_MASK) {
283 case BNEP_EXT_CONTROL:
284 bnep_rx_control(s, skb->data, skb->len);
285 break;
286
287 default:
288 /* Unknown extension, skip it. */
289 break;
290 }
291
292 if (!skb_pull(skb, h->len)) {
293 err = -EILSEQ;
294 break;
295 }
296 } while (!err && (h->type & BNEP_EXT_HEADER));
297
298 return err;
299}
300
301static u8 __bnep_rx_hlen[] = {
302 ETH_HLEN, /* BNEP_GENERAL */
303 0, /* BNEP_CONTROL */
304 2, /* BNEP_COMPRESSED */
305 ETH_ALEN + 2, /* BNEP_COMPRESSED_SRC_ONLY */
306 ETH_ALEN + 2 /* BNEP_COMPRESSED_DST_ONLY */
307};
308
309static inline int bnep_rx_frame(struct bnep_session *s, struct sk_buff *skb)
310{
311 struct net_device *dev = s->dev;
312 struct sk_buff *nskb;
313 u8 type;
314
315 dev->stats.rx_bytes += skb->len;
316
317 type = *(u8 *) skb->data;
318 skb_pull(skb, 1);
319
320 if ((type & BNEP_TYPE_MASK) >= sizeof(__bnep_rx_hlen))
321 goto badframe;
322
323 if ((type & BNEP_TYPE_MASK) == BNEP_CONTROL) {
324 bnep_rx_control(s, skb->data, skb->len);
325 kfree_skb(skb);
326 return 0;
327 }
328
329 skb_reset_mac_header(skb);
330
331 /* Verify and pull out header */
332 if (!skb_pull(skb, __bnep_rx_hlen[type & BNEP_TYPE_MASK]))
333 goto badframe;
334
335 s->eh.h_proto = get_unaligned((__be16 *) (skb->data - 2));
336
337 if (type & BNEP_EXT_HEADER) {
338 if (bnep_rx_extension(s, skb) < 0)
339 goto badframe;
340 }
341
342 /* Strip 802.1p header */
343 if (ntohs(s->eh.h_proto) == ETH_P_8021Q) {
344 if (!skb_pull(skb, 4))
345 goto badframe;
346 s->eh.h_proto = get_unaligned((__be16 *) (skb->data - 2));
347 }
348
349 /* We have to alloc new skb and copy data here :(. Because original skb
350 * may not be modified and because of the alignment requirements. */
351 nskb = alloc_skb(2 + ETH_HLEN + skb->len, GFP_KERNEL);
352 if (!nskb) {
353 dev->stats.rx_dropped++;
354 kfree_skb(skb);
355 return -ENOMEM;
356 }
357 skb_reserve(nskb, 2);
358
359 /* Decompress header and construct ether frame */
360 switch (type & BNEP_TYPE_MASK) {
361 case BNEP_COMPRESSED:
362 memcpy(__skb_put(nskb, ETH_HLEN), &s->eh, ETH_HLEN);
363 break;
364
365 case BNEP_COMPRESSED_SRC_ONLY:
366 memcpy(__skb_put(nskb, ETH_ALEN), s->eh.h_dest, ETH_ALEN);
367 memcpy(__skb_put(nskb, ETH_ALEN), skb_mac_header(skb), ETH_ALEN);
368 put_unaligned(s->eh.h_proto, (__be16 *) __skb_put(nskb, 2));
369 break;
370
371 case BNEP_COMPRESSED_DST_ONLY:
372 memcpy(__skb_put(nskb, ETH_ALEN), skb_mac_header(skb),
373 ETH_ALEN);
374 memcpy(__skb_put(nskb, ETH_ALEN + 2), s->eh.h_source,
375 ETH_ALEN + 2);
376 break;
377
378 case BNEP_GENERAL:
379 memcpy(__skb_put(nskb, ETH_ALEN * 2), skb_mac_header(skb),
380 ETH_ALEN * 2);
381 put_unaligned(s->eh.h_proto, (__be16 *) __skb_put(nskb, 2));
382 break;
383 }
384
385 skb_copy_from_linear_data(skb, __skb_put(nskb, skb->len), skb->len);
386 kfree_skb(skb);
387
388 dev->stats.rx_packets++;
389 nskb->ip_summed = CHECKSUM_NONE;
390 nskb->protocol = eth_type_trans(nskb, dev);
391 netif_rx_ni(nskb);
392 return 0;
393
394badframe:
395 dev->stats.rx_errors++;
396 kfree_skb(skb);
397 return 0;
398}
399
400static u8 __bnep_tx_types[] = {
401 BNEP_GENERAL,
402 BNEP_COMPRESSED_SRC_ONLY,
403 BNEP_COMPRESSED_DST_ONLY,
404 BNEP_COMPRESSED
405};
406
407static inline int bnep_tx_frame(struct bnep_session *s, struct sk_buff *skb)
408{
409 struct ethhdr *eh = (void *) skb->data;
410 struct socket *sock = s->sock;
411 struct kvec iv[3];
412 int len = 0, il = 0;
413 u8 type = 0;
414
415 BT_DBG("skb %p dev %p type %d", skb, skb->dev, skb->pkt_type);
416
417 if (!skb->dev) {
418 /* Control frame sent by us */
419 goto send;
420 }
421
422 iv[il++] = (struct kvec) { &type, 1 };
423 len++;
424
425 if (compress_src && ether_addr_equal(eh->h_dest, s->eh.h_source))
426 type |= 0x01;
427
428 if (compress_dst && ether_addr_equal(eh->h_source, s->eh.h_dest))
429 type |= 0x02;
430
431 if (type)
432 skb_pull(skb, ETH_ALEN * 2);
433
434 type = __bnep_tx_types[type];
435 switch (type) {
436 case BNEP_COMPRESSED_SRC_ONLY:
437 iv[il++] = (struct kvec) { eh->h_source, ETH_ALEN };
438 len += ETH_ALEN;
439 break;
440
441 case BNEP_COMPRESSED_DST_ONLY:
442 iv[il++] = (struct kvec) { eh->h_dest, ETH_ALEN };
443 len += ETH_ALEN;
444 break;
445 }
446
447send:
448 iv[il++] = (struct kvec) { skb->data, skb->len };
449 len += skb->len;
450
451 /* FIXME: linearize skb */
452 {
453 len = kernel_sendmsg(sock, &s->msg, iv, il, len);
454 }
455 kfree_skb(skb);
456
457 if (len > 0) {
458 s->dev->stats.tx_bytes += len;
459 s->dev->stats.tx_packets++;
460 return 0;
461 }
462
463 return len;
464}
465
466static int bnep_session(void *arg)
467{
468 struct bnep_session *s = arg;
469 struct net_device *dev = s->dev;
470 struct sock *sk = s->sock->sk;
471 struct sk_buff *skb;
472 wait_queue_t wait;
473
474 BT_DBG("");
475
476 set_user_nice(current, -15);
477
478 init_waitqueue_entry(&wait, current);
479 add_wait_queue(sk_sleep(sk), &wait);
480 while (1) {
481 set_current_state(TASK_INTERRUPTIBLE);
482
483 if (atomic_read(&s->terminate))
484 break;
485 /* RX */
486 while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
487 skb_orphan(skb);
488 if (!skb_linearize(skb))
489 bnep_rx_frame(s, skb);
490 else
491 kfree_skb(skb);
492 }
493
494 if (sk->sk_state != BT_CONNECTED)
495 break;
496
497 /* TX */
498 while ((skb = skb_dequeue(&sk->sk_write_queue)))
499 if (bnep_tx_frame(s, skb))
500 break;
501 netif_wake_queue(dev);
502
503 schedule();
504 }
505 __set_current_state(TASK_RUNNING);
506 remove_wait_queue(sk_sleep(sk), &wait);
507
508 /* Cleanup session */
509 down_write(&bnep_session_sem);
510
511 /* Delete network device */
512 unregister_netdev(dev);
513
514 /* Wakeup user-space polling for socket errors */
515 s->sock->sk->sk_err = EUNATCH;
516
517 wake_up_interruptible(sk_sleep(s->sock->sk));
518
519 /* Release the socket */
520 fput(s->sock->file);
521
522 __bnep_unlink_session(s);
523
524 up_write(&bnep_session_sem);
525 free_netdev(dev);
526 module_put_and_exit(0);
527 return 0;
528}
529
530static struct device *bnep_get_device(struct bnep_session *session)
531{
532 bdaddr_t *src = &bt_sk(session->sock->sk)->src;
533 bdaddr_t *dst = &bt_sk(session->sock->sk)->dst;
534 struct hci_dev *hdev;
535 struct hci_conn *conn;
536
537 hdev = hci_get_route(dst, src);
538 if (!hdev)
539 return NULL;
540
541 conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, dst);
542
543 hci_dev_put(hdev);
544
545 return conn ? &conn->dev : NULL;
546}
547
548static struct device_type bnep_type = {
549 .name = "bluetooth",
550};
551
552int bnep_add_connection(struct bnep_connadd_req *req, struct socket *sock)
553{
554 struct net_device *dev;
555 struct bnep_session *s, *ss;
556 u8 dst[ETH_ALEN], src[ETH_ALEN];
557 int err;
558
559 BT_DBG("");
560
561 baswap((void *) dst, &bt_sk(sock->sk)->dst);
562 baswap((void *) src, &bt_sk(sock->sk)->src);
563
564 /* session struct allocated as private part of net_device */
565 dev = alloc_netdev(sizeof(struct bnep_session),
566 (*req->device) ? req->device : "bnep%d",
567 bnep_net_setup);
568 if (!dev)
569 return -ENOMEM;
570
571 down_write(&bnep_session_sem);
572
573 ss = __bnep_get_session(dst);
574 if (ss && ss->state == BT_CONNECTED) {
575 err = -EEXIST;
576 goto failed;
577 }
578
579 s = netdev_priv(dev);
580
581 /* This is rx header therefore addresses are swapped.
582 * ie. eh.h_dest is our local address. */
583 memcpy(s->eh.h_dest, &src, ETH_ALEN);
584 memcpy(s->eh.h_source, &dst, ETH_ALEN);
585 memcpy(dev->dev_addr, s->eh.h_dest, ETH_ALEN);
586
587 s->dev = dev;
588 s->sock = sock;
589 s->role = req->role;
590 s->state = BT_CONNECTED;
591
592 s->msg.msg_flags = MSG_NOSIGNAL;
593
594#ifdef CONFIG_BT_BNEP_MC_FILTER
595 /* Set default mc filter */
596 set_bit(bnep_mc_hash(dev->broadcast), (ulong *) &s->mc_filter);
597#endif
598
599#ifdef CONFIG_BT_BNEP_PROTO_FILTER
600 /* Set default protocol filter */
601 bnep_set_default_proto_filter(s);
602#endif
603
604 SET_NETDEV_DEV(dev, bnep_get_device(s));
605 SET_NETDEV_DEVTYPE(dev, &bnep_type);
606
607 err = register_netdev(dev);
608 if (err)
609 goto failed;
610
611 __bnep_link_session(s);
612
613 __module_get(THIS_MODULE);
614 s->task = kthread_run(bnep_session, s, "kbnepd %s", dev->name);
615 if (IS_ERR(s->task)) {
616 /* Session thread start failed, gotta cleanup. */
617 module_put(THIS_MODULE);
618 unregister_netdev(dev);
619 __bnep_unlink_session(s);
620 err = PTR_ERR(s->task);
621 goto failed;
622 }
623
624 up_write(&bnep_session_sem);
625 strcpy(req->device, dev->name);
626 return 0;
627
628failed:
629 up_write(&bnep_session_sem);
630 free_netdev(dev);
631 return err;
632}
633
634int bnep_del_connection(struct bnep_conndel_req *req)
635{
636 struct bnep_session *s;
637 int err = 0;
638
639 BT_DBG("");
640
641 down_read(&bnep_session_sem);
642
643 s = __bnep_get_session(req->dst);
644 if (s) {
645 atomic_inc(&s->terminate);
646 wake_up_process(s->task);
647 } else
648 err = -ENOENT;
649
650 up_read(&bnep_session_sem);
651 return err;
652}
653
654static void __bnep_copy_ci(struct bnep_conninfo *ci, struct bnep_session *s)
655{
656 memset(ci, 0, sizeof(*ci));
657 memcpy(ci->dst, s->eh.h_source, ETH_ALEN);
658 strcpy(ci->device, s->dev->name);
659 ci->flags = s->flags;
660 ci->state = s->state;
661 ci->role = s->role;
662}
663
664int bnep_get_connlist(struct bnep_connlist_req *req)
665{
666 struct bnep_session *s;
667 int err = 0, n = 0;
668
669 down_read(&bnep_session_sem);
670
671 list_for_each_entry(s, &bnep_session_list, list) {
672 struct bnep_conninfo ci;
673
674 __bnep_copy_ci(&ci, s);
675
676 if (copy_to_user(req->ci, &ci, sizeof(ci))) {
677 err = -EFAULT;
678 break;
679 }
680
681 if (++n >= req->cnum)
682 break;
683
684 req->ci++;
685 }
686 req->cnum = n;
687
688 up_read(&bnep_session_sem);
689 return err;
690}
691
692int bnep_get_conninfo(struct bnep_conninfo *ci)
693{
694 struct bnep_session *s;
695 int err = 0;
696
697 down_read(&bnep_session_sem);
698
699 s = __bnep_get_session(ci->dst);
700 if (s)
701 __bnep_copy_ci(ci, s);
702 else
703 err = -ENOENT;
704
705 up_read(&bnep_session_sem);
706 return err;
707}
708
709static int __init bnep_init(void)
710{
711 char flt[50] = "";
712
713#ifdef CONFIG_BT_BNEP_PROTO_FILTER
714 strcat(flt, "protocol ");
715#endif
716
717#ifdef CONFIG_BT_BNEP_MC_FILTER
718 strcat(flt, "multicast");
719#endif
720
721 BT_INFO("BNEP (Ethernet Emulation) ver %s", VERSION);
722 if (flt[0])
723 BT_INFO("BNEP filters: %s", flt);
724
725 bnep_sock_init();
726 return 0;
727}
728
729static void __exit bnep_exit(void)
730{
731 bnep_sock_cleanup();
732}
733
734module_init(bnep_init);
735module_exit(bnep_exit);
736
737module_param(compress_src, bool, 0644);
738MODULE_PARM_DESC(compress_src, "Compress sources headers");
739
740module_param(compress_dst, bool, 0644);
741MODULE_PARM_DESC(compress_dst, "Compress destination headers");
742
743MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
744MODULE_DESCRIPTION("Bluetooth BNEP ver " VERSION);
745MODULE_VERSION(VERSION);
746MODULE_LICENSE("GPL");
747MODULE_ALIAS("bt-proto-4");