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
493 smp_mb__before_atomic();
494
495 if (atomic_read(&s->terminate))
496 break;
497
498 while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
499 skb_orphan(skb);
500 if (!skb_linearize(skb))
501 bnep_rx_frame(s, skb);
502 else
503 kfree_skb(skb);
504 }
505
506 if (sk->sk_state != BT_CONNECTED)
507 break;
508
509
510 while ((skb = skb_dequeue(&sk->sk_write_queue)))
511 if (bnep_tx_frame(s, skb))
512 break;
513 netif_wake_queue(dev);
514
515 wait_woken(&wait, TASK_INTERRUPTIBLE, MAX_SCHEDULE_TIMEOUT);
516 }
517 remove_wait_queue(sk_sleep(sk), &wait);
518
519
520 down_write(&bnep_session_sem);
521
522
523 unregister_netdev(dev);
524
525
526 s->sock->sk->sk_err = EUNATCH;
527
528 wake_up_interruptible(sk_sleep(s->sock->sk));
529
530
531 fput(s->sock->file);
532
533 __bnep_unlink_session(s);
534
535 up_write(&bnep_session_sem);
536 free_netdev(dev);
537 module_put_and_exit(0);
538 return 0;
539}
540
541static struct device *bnep_get_device(struct bnep_session *session)
542{
543 struct l2cap_conn *conn = l2cap_pi(session->sock->sk)->chan->conn;
544
545 if (!conn || !conn->hcon)
546 return NULL;
547
548 return &conn->hcon->dev;
549}
550
551static struct device_type bnep_type = {
552 .name = "bluetooth",
553};
554
555int bnep_add_connection(struct bnep_connadd_req *req, struct socket *sock)
556{
557 u32 valid_flags = BIT(BNEP_SETUP_RESPONSE);
558 struct net_device *dev;
559 struct bnep_session *s, *ss;
560 u8 dst[ETH_ALEN], src[ETH_ALEN];
561 int err;
562
563 BT_DBG("");
564
565 if (!l2cap_is_socket(sock))
566 return -EBADFD;
567
568 if (req->flags & ~valid_flags)
569 return -EINVAL;
570
571 baswap((void *) dst, &l2cap_pi(sock->sk)->chan->dst);
572 baswap((void *) src, &l2cap_pi(sock->sk)->chan->src);
573
574
575 dev = alloc_netdev(sizeof(struct bnep_session),
576 (*req->device) ? req->device : "bnep%d",
577 NET_NAME_UNKNOWN,
578 bnep_net_setup);
579 if (!dev)
580 return -ENOMEM;
581
582 down_write(&bnep_session_sem);
583
584 ss = __bnep_get_session(dst);
585 if (ss && ss->state == BT_CONNECTED) {
586 err = -EEXIST;
587 goto failed;
588 }
589
590 s = netdev_priv(dev);
591
592
593
594 memcpy(s->eh.h_dest, &src, ETH_ALEN);
595 memcpy(s->eh.h_source, &dst, ETH_ALEN);
596 memcpy(dev->dev_addr, s->eh.h_dest, ETH_ALEN);
597
598 s->dev = dev;
599 s->sock = sock;
600 s->role = req->role;
601 s->state = BT_CONNECTED;
602 s->flags = req->flags;
603
604 s->msg.msg_flags = MSG_NOSIGNAL;
605
606#ifdef CONFIG_BT_BNEP_MC_FILTER
607
608
609
610
611 s->mc_filter = ~0LL;
612#endif
613
614#ifdef CONFIG_BT_BNEP_PROTO_FILTER
615
616 bnep_set_default_proto_filter(s);
617#endif
618
619 SET_NETDEV_DEV(dev, bnep_get_device(s));
620 SET_NETDEV_DEVTYPE(dev, &bnep_type);
621
622 err = register_netdev(dev);
623 if (err)
624 goto failed;
625
626 __bnep_link_session(s);
627
628 __module_get(THIS_MODULE);
629 s->task = kthread_run(bnep_session, s, "kbnepd %s", dev->name);
630 if (IS_ERR(s->task)) {
631
632 module_put(THIS_MODULE);
633 unregister_netdev(dev);
634 __bnep_unlink_session(s);
635 err = PTR_ERR(s->task);
636 goto failed;
637 }
638
639 up_write(&bnep_session_sem);
640 strcpy(req->device, dev->name);
641 return 0;
642
643failed:
644 up_write(&bnep_session_sem);
645 free_netdev(dev);
646 return err;
647}
648
649int bnep_del_connection(struct bnep_conndel_req *req)
650{
651 u32 valid_flags = 0;
652 struct bnep_session *s;
653 int err = 0;
654
655 BT_DBG("");
656
657 if (req->flags & ~valid_flags)
658 return -EINVAL;
659
660 down_read(&bnep_session_sem);
661
662 s = __bnep_get_session(req->dst);
663 if (s) {
664 atomic_inc(&s->terminate);
665 wake_up_interruptible(sk_sleep(s->sock->sk));
666 } else
667 err = -ENOENT;
668
669 up_read(&bnep_session_sem);
670 return err;
671}
672
673static void __bnep_copy_ci(struct bnep_conninfo *ci, struct bnep_session *s)
674{
675 u32 valid_flags = BIT(BNEP_SETUP_RESPONSE);
676
677 memset(ci, 0, sizeof(*ci));
678 memcpy(ci->dst, s->eh.h_source, ETH_ALEN);
679 strcpy(ci->device, s->dev->name);
680 ci->flags = s->flags & valid_flags;
681 ci->state = s->state;
682 ci->role = s->role;
683}
684
685int bnep_get_connlist(struct bnep_connlist_req *req)
686{
687 struct bnep_session *s;
688 int err = 0, n = 0;
689
690 down_read(&bnep_session_sem);
691
692 list_for_each_entry(s, &bnep_session_list, list) {
693 struct bnep_conninfo ci;
694
695 __bnep_copy_ci(&ci, s);
696
697 if (copy_to_user(req->ci, &ci, sizeof(ci))) {
698 err = -EFAULT;
699 break;
700 }
701
702 if (++n >= req->cnum)
703 break;
704
705 req->ci++;
706 }
707 req->cnum = n;
708
709 up_read(&bnep_session_sem);
710 return err;
711}
712
713int bnep_get_conninfo(struct bnep_conninfo *ci)
714{
715 struct bnep_session *s;
716 int err = 0;
717
718 down_read(&bnep_session_sem);
719
720 s = __bnep_get_session(ci->dst);
721 if (s)
722 __bnep_copy_ci(ci, s);
723 else
724 err = -ENOENT;
725
726 up_read(&bnep_session_sem);
727 return err;
728}
729
730static int __init bnep_init(void)
731{
732 char flt[50] = "";
733
734#ifdef CONFIG_BT_BNEP_PROTO_FILTER
735 strcat(flt, "protocol ");
736#endif
737
738#ifdef CONFIG_BT_BNEP_MC_FILTER
739 strcat(flt, "multicast");
740#endif
741
742 BT_INFO("BNEP (Ethernet Emulation) ver %s", VERSION);
743 if (flt[0])
744 BT_INFO("BNEP filters: %s", flt);
745
746 bnep_sock_init();
747 return 0;
748}
749
750static void __exit bnep_exit(void)
751{
752 bnep_sock_cleanup();
753}
754
755module_init(bnep_init);
756module_exit(bnep_exit);
757
758module_param(compress_src, bool, 0644);
759MODULE_PARM_DESC(compress_src, "Compress sources headers");
760
761module_param(compress_dst, bool, 0644);
762MODULE_PARM_DESC(compress_dst, "Compress destination headers");
763
764MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
765MODULE_DESCRIPTION("Bluetooth BNEP ver " VERSION);
766MODULE_VERSION(VERSION);
767MODULE_LICENSE("GPL");
768MODULE_ALIAS("bt-proto-4");
769
