1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
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
92 s->proto_filter[0].start = ETH_P_IP;
93 s->proto_filter[0].end = ETH_P_ARP;
94
95 s->proto_filter[1].start = ETH_P_RARP;
96 s->proto_filter[1].end = ETH_P_AARP;
97
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
174 set_bit(bnep_mc_hash(s->dev->broadcast), (ulong *) &s->mc_filter);
175
176
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
188 set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter);
189 while (memcmp(a1, a2, 6) < 0 && s->mc_filter != ~0LL) {
190
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
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
235 if (test_bit(BNEP_SETUP_RESPONSE, &s->flags) &&
236 !test_and_set_bit(BNEP_SETUP_RSP_SENT, &s->flags))
237 err = bnep_send_rsp(s, BNEP_SETUP_CONN_RSP,
238 BNEP_SUCCESS);
239 else
240 err = bnep_send_rsp(s, BNEP_SETUP_CONN_RSP,
241 BNEP_CONN_NOT_ALLOWED);
242 break;
243
244 default: {
245 u8 pkt[3];
246 pkt[0] = BNEP_CONTROL;
247 pkt[1] = BNEP_CMD_NOT_UNDERSTOOD;
248 pkt[2] = cmd;
249 err = bnep_send(s, pkt, sizeof(pkt));
250 }
251 break;
252 }
253
254 return err;
255}
256
257static int bnep_rx_extension(struct bnep_session *s, struct sk_buff *skb)
258{
259 struct bnep_ext_hdr *h;
260 int err = 0;
261
262 do {
263 h = (void *) skb->data;
264 if (!skb_pull(skb, sizeof(*h))) {
265 err = -EILSEQ;
266 break;
267 }
268
269 BT_DBG("type 0x%x len %d", h->type, h->len);
270
271 switch (h->type & BNEP_TYPE_MASK) {
272 case BNEP_EXT_CONTROL:
273 bnep_rx_control(s, skb->data, skb->len);
274 break;
275
276 default:
277
278 break;
279 }
280
281 if (!skb_pull(skb, h->len)) {
282 err = -EILSEQ;
283 break;
284 }
285 } while (!err && (h->type & BNEP_EXT_HEADER));
286
287 return err;
288}
289
290static u8 __bnep_rx_hlen[] = {
291 ETH_HLEN,
292 0,
293 2,
294 ETH_ALEN + 2,
295 ETH_ALEN + 2
296};
297
298static int bnep_rx_frame(struct bnep_session *s, struct sk_buff *skb)
299{
300 struct net_device *dev = s->dev;
301 struct sk_buff *nskb;
302 u8 type, ctrl_type;
303
304 dev->stats.rx_bytes += skb->len;
305
306 type = *(u8 *) skb->data;
307 skb_pull(skb, 1);
308 ctrl_type = *(u8 *)skb->data;
309
310 if ((type & BNEP_TYPE_MASK) >= sizeof(__bnep_rx_hlen))
311 goto badframe;
312
313 if ((type & BNEP_TYPE_MASK) == BNEP_CONTROL) {
314 if (bnep_rx_control(s, skb->data, skb->len) < 0) {
315 dev->stats.tx_errors++;
316 kfree_skb(skb);
317 return 0;
318 }
319
320 if (!(type & BNEP_EXT_HEADER)) {
321 kfree_skb(skb);
322 return 0;
323 }
324
325
326 switch (ctrl_type) {
327 case BNEP_SETUP_CONN_REQ:
328
329 if (!skb_pull(skb, 2 + *(u8 *)(skb->data + 1) * 2))
330 goto badframe;
331 break;
332 case BNEP_FILTER_MULTI_ADDR_SET:
333 case BNEP_FILTER_NET_TYPE_SET:
334
335 if (!skb_pull(skb, 3 + *(u16 *)(skb->data + 1) * 2))
336 goto badframe;
337 break;
338 default:
339 kfree_skb(skb);
340 return 0;
341 }
342 } else {
343 skb_reset_mac_header(skb);
344
345
346 if (!skb_pull(skb, __bnep_rx_hlen[type & BNEP_TYPE_MASK]))
347 goto badframe;
348
349 s->eh.h_proto = get_unaligned((__be16 *) (skb->data - 2));
350 }
351
352 if (type & BNEP_EXT_HEADER) {
353 if (bnep_rx_extension(s, skb) < 0)
354 goto badframe;
355 }
356
357
358 if (ntohs(s->eh.h_proto) == ETH_P_8021Q) {
359 if (!skb_pull(skb, 4))
360 goto badframe;
361 s->eh.h_proto = get_unaligned((__be16 *) (skb->data - 2));
362 }
363
364
365
366 nskb = alloc_skb(2 + ETH_HLEN + skb->len, GFP_KERNEL);
367 if (!nskb) {
368 dev->stats.rx_dropped++;
369 kfree_skb(skb);
370 return -ENOMEM;
371 }
372 skb_reserve(nskb, 2);
373
374
375 switch (type & BNEP_TYPE_MASK) {
376 case BNEP_COMPRESSED:
377 __skb_put_data(nskb, &s->eh, ETH_HLEN);
378 break;
379
380 case BNEP_COMPRESSED_SRC_ONLY:
381 __skb_put_data(nskb, s->eh.h_dest, ETH_ALEN);
382 __skb_put_data(nskb, skb_mac_header(skb), ETH_ALEN);
383 put_unaligned(s->eh.h_proto, (__be16 *) __skb_put(nskb, 2));
384 break;
385
386 case BNEP_COMPRESSED_DST_ONLY:
387 __skb_put_data(nskb, skb_mac_header(skb), ETH_ALEN);
388 __skb_put_data(nskb, s->eh.h_source, ETH_ALEN + 2);
389 break;
390
391 case BNEP_GENERAL:
392 __skb_put_data(nskb, skb_mac_header(skb), ETH_ALEN * 2);
393 put_unaligned(s->eh.h_proto, (__be16 *) __skb_put(nskb, 2));
394 break;
395 }
396
397 skb_copy_from_linear_data(skb, __skb_put(nskb, skb->len), skb->len);
398 kfree_skb(skb);
399
400 dev->stats.rx_packets++;
401 nskb->ip_summed = CHECKSUM_NONE;
402 nskb->protocol = eth_type_trans(nskb, dev);
403 netif_rx_ni(nskb);
404 return 0;
405
406badframe:
407 dev->stats.rx_errors++;
408 kfree_skb(skb);
409 return 0;
410}
411
412static u8 __bnep_tx_types[] = {
413 BNEP_GENERAL,
414 BNEP_COMPRESSED_SRC_ONLY,
415 BNEP_COMPRESSED_DST_ONLY,
416 BNEP_COMPRESSED
417};
418
419static int bnep_tx_frame(struct bnep_session *s, struct sk_buff *skb)
420{
421 struct ethhdr *eh = (void *) skb->data;
422 struct socket *sock = s->sock;
423 struct kvec iv[3];
424 int len = 0, il = 0;
425 u8 type = 0;
426
427 BT_DBG("skb %p dev %p type %d", skb, skb->dev, skb->pkt_type);
428
429 if (!skb->dev) {
430
431 goto send;
432 }
433
434 iv[il++] = (struct kvec) { &type, 1 };
435 len++;
436
437 if (compress_src && ether_addr_equal(eh->h_dest, s->eh.h_source))
438 type |= 0x01;
439
440 if (compress_dst && ether_addr_equal(eh->h_source, s->eh.h_dest))
441 type |= 0x02;
442
443 if (type)
444 skb_pull(skb, ETH_ALEN * 2);
445
446 type = __bnep_tx_types[type];
447 switch (type) {
448 case BNEP_COMPRESSED_SRC_ONLY:
449 iv[il++] = (struct kvec) { eh->h_source, ETH_ALEN };
450 len += ETH_ALEN;
451 break;
452
453 case BNEP_COMPRESSED_DST_ONLY:
454 iv[il++] = (struct kvec) { eh->h_dest, ETH_ALEN };
455 len += ETH_ALEN;
456 break;
457 }
458
459send:
460 iv[il++] = (struct kvec) { skb->data, skb->len };
461 len += skb->len;
462
463
464 {
465 len = kernel_sendmsg(sock, &s->msg, iv, il, len);
466 }
467 kfree_skb(skb);
468
469 if (len > 0) {
470 s->dev->stats.tx_bytes += len;
471 s->dev->stats.tx_packets++;
472 return 0;
473 }
474
475 return len;
476}
477
478static int bnep_session(void *arg)
479{
480 struct bnep_session *s = arg;
481 struct net_device *dev = s->dev;
482 struct sock *sk = s->sock->sk;
483 struct sk_buff *skb;
484 DEFINE_WAIT_FUNC(wait, woken_wake_function);
485
486 BT_DBG("");
487
488 set_user_nice(current, -15);
489
490 add_wait_queue(sk_sleep(sk), &wait);
491 while (1) {
492 if (atomic_read(&s->terminate))
493 break;
494
495 while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
496 skb_orphan(skb);
497 if (!skb_linearize(skb))
498 bnep_rx_frame(s, skb);
499 else
500 kfree_skb(skb);
501 }
502
503 if (sk->sk_state != BT_CONNECTED)
504 break;
505
506
507 while ((skb = skb_dequeue(&sk->sk_write_queue)))
508 if (bnep_tx_frame(s, skb))
509 break;
510 netif_wake_queue(dev);
511
512
513
514
515
516 wait_woken(&wait, TASK_INTERRUPTIBLE, MAX_SCHEDULE_TIMEOUT);
517 }
518 remove_wait_queue(sk_sleep(sk), &wait);
519
520
521 down_write(&bnep_session_sem);
522
523
524 unregister_netdev(dev);
525
526
527 s->sock->sk->sk_err = EUNATCH;
528
529 wake_up_interruptible(sk_sleep(s->sock->sk));
530
531
532 fput(s->sock->file);
533
534 __bnep_unlink_session(s);
535
536 up_write(&bnep_session_sem);
537 free_netdev(dev);
538 module_put_and_exit(0);
539 return 0;
540}
541
542static struct device *bnep_get_device(struct bnep_session *session)
543{
544 struct l2cap_conn *conn = l2cap_pi(session->sock->sk)->chan->conn;
545
546 if (!conn || !conn->hcon)
547 return NULL;
548
549 return &conn->hcon->dev;
550}
551
552static struct device_type bnep_type = {
553 .name = "bluetooth",
554};
555
556int bnep_add_connection(struct bnep_connadd_req *req, struct socket *sock)
557{
558 u32 valid_flags = BIT(BNEP_SETUP_RESPONSE);
559 struct net_device *dev;
560 struct bnep_session *s, *ss;
561 u8 dst[ETH_ALEN], src[ETH_ALEN];
562 int err;
563
564 BT_DBG("");
565
566 if (!l2cap_is_socket(sock))
567 return -EBADFD;
568
569 if (req->flags & ~valid_flags)
570 return -EINVAL;
571
572 baswap((void *) dst, &l2cap_pi(sock->sk)->chan->dst);
573 baswap((void *) src, &l2cap_pi(sock->sk)->chan->src);
574
575
576 dev = alloc_netdev(sizeof(struct bnep_session),
577 (*req->device) ? req->device : "bnep%d",
578 NET_NAME_UNKNOWN,
579 bnep_net_setup);
580 if (!dev)
581 return -ENOMEM;
582
583 down_write(&bnep_session_sem);
584
585 ss = __bnep_get_session(dst);
586 if (ss && ss->state == BT_CONNECTED) {
587 err = -EEXIST;
588 goto failed;
589 }
590
591 s = netdev_priv(dev);
592
593
594
595 memcpy(s->eh.h_dest, &src, ETH_ALEN);
596 memcpy(s->eh.h_source, &dst, ETH_ALEN);
597 memcpy(dev->dev_addr, s->eh.h_dest, ETH_ALEN);
598
599 s->dev = dev;
600 s->sock = sock;
601 s->role = req->role;
602 s->state = BT_CONNECTED;
603 s->flags = req->flags;
604
605 s->msg.msg_flags = MSG_NOSIGNAL;
606
607#ifdef CONFIG_BT_BNEP_MC_FILTER
608
609
610
611
612 s->mc_filter = ~0LL;
613#endif
614
615#ifdef CONFIG_BT_BNEP_PROTO_FILTER
616
617 bnep_set_default_proto_filter(s);
618#endif
619
620 SET_NETDEV_DEV(dev, bnep_get_device(s));
621 SET_NETDEV_DEVTYPE(dev, &bnep_type);
622
623 err = register_netdev(dev);
624 if (err)
625 goto failed;
626
627 __bnep_link_session(s);
628
629 __module_get(THIS_MODULE);
630 s->task = kthread_run(bnep_session, s, "kbnepd %s", dev->name);
631 if (IS_ERR(s->task)) {
632
633 module_put(THIS_MODULE);
634 unregister_netdev(dev);
635 __bnep_unlink_session(s);
636 err = PTR_ERR(s->task);
637 goto failed;
638 }
639
640 up_write(&bnep_session_sem);
641 strcpy(req->device, dev->name);
642 return 0;
643
644failed:
645 up_write(&bnep_session_sem);
646 free_netdev(dev);
647 return err;
648}
649
650int bnep_del_connection(struct bnep_conndel_req *req)
651{
652 u32 valid_flags = 0;
653 struct bnep_session *s;
654 int err = 0;
655
656 BT_DBG("");
657
658 if (req->flags & ~valid_flags)
659 return -EINVAL;
660
661 down_read(&bnep_session_sem);
662
663 s = __bnep_get_session(req->dst);
664 if (s) {
665 atomic_inc(&s->terminate);
666 wake_up_interruptible(sk_sleep(s->sock->sk));
667 } else
668 err = -ENOENT;
669
670 up_read(&bnep_session_sem);
671 return err;
672}
673
674static void __bnep_copy_ci(struct bnep_conninfo *ci, struct bnep_session *s)
675{
676 u32 valid_flags = BIT(BNEP_SETUP_RESPONSE);
677
678 memset(ci, 0, sizeof(*ci));
679 memcpy(ci->dst, s->eh.h_source, ETH_ALEN);
680 strcpy(ci->device, s->dev->name);
681 ci->flags = s->flags & valid_flags;
682 ci->state = s->state;
683 ci->role = s->role;
684}
685
686int bnep_get_connlist(struct bnep_connlist_req *req)
687{
688 struct bnep_session *s;
689 int err = 0, n = 0;
690
691 down_read(&bnep_session_sem);
692
693 list_for_each_entry(s, &bnep_session_list, list) {
694 struct bnep_conninfo ci;
695
696 __bnep_copy_ci(&ci, s);
697
698 if (copy_to_user(req->ci, &ci, sizeof(ci))) {
699 err = -EFAULT;
700 break;
701 }
702
703 if (++n >= req->cnum)
704 break;
705
706 req->ci++;
707 }
708 req->cnum = n;
709
710 up_read(&bnep_session_sem);
711 return err;
712}
713
714int bnep_get_conninfo(struct bnep_conninfo *ci)
715{
716 struct bnep_session *s;
717 int err = 0;
718
719 down_read(&bnep_session_sem);
720
721 s = __bnep_get_session(ci->dst);
722 if (s)
723 __bnep_copy_ci(ci, s);
724 else
725 err = -ENOENT;
726
727 up_read(&bnep_session_sem);
728 return err;
729}
730
731static int __init bnep_init(void)
732{
733 char flt[50] = "";
734
735#ifdef CONFIG_BT_BNEP_PROTO_FILTER
736 strcat(flt, "protocol ");
737#endif
738
739#ifdef CONFIG_BT_BNEP_MC_FILTER
740 strcat(flt, "multicast");
741#endif
742
743 BT_INFO("BNEP (Ethernet Emulation) ver %s", VERSION);
744 if (flt[0])
745 BT_INFO("BNEP filters: %s", flt);
746
747 bnep_sock_init();
748 return 0;
749}
750
751static void __exit bnep_exit(void)
752{
753 bnep_sock_cleanup();
754}
755
756module_init(bnep_init);
757module_exit(bnep_exit);
758
759module_param(compress_src, bool, 0644);
760MODULE_PARM_DESC(compress_src, "Compress sources headers");
761
762module_param(compress_dst, bool, 0644);
763MODULE_PARM_DESC(compress_dst, "Compress destination headers");
764
765MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
766MODULE_DESCRIPTION("Bluetooth BNEP ver " VERSION);
767MODULE_VERSION(VERSION);
768MODULE_LICENSE("GPL");
769MODULE_ALIAS("bt-proto-4");
770
