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 memcpy(__skb_put(nskb, ETH_HLEN), &s->eh, ETH_HLEN);
378 break;
379
380 case BNEP_COMPRESSED_SRC_ONLY:
381 memcpy(__skb_put(nskb, ETH_ALEN), s->eh.h_dest, ETH_ALEN);
382 memcpy(__skb_put(nskb, ETH_ALEN), 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 memcpy(__skb_put(nskb, ETH_ALEN), skb_mac_header(skb),
388 ETH_ALEN);
389 memcpy(__skb_put(nskb, ETH_ALEN + 2), s->eh.h_source,
390 ETH_ALEN + 2);
391 break;
392
393 case BNEP_GENERAL:
394 memcpy(__skb_put(nskb, ETH_ALEN * 2), skb_mac_header(skb),
395 ETH_ALEN * 2);
396 put_unaligned(s->eh.h_proto, (__be16 *) __skb_put(nskb, 2));
397 break;
398 }
399
400 skb_copy_from_linear_data(skb, __skb_put(nskb, skb->len), skb->len);
401 kfree_skb(skb);
402
403 dev->stats.rx_packets++;
404 nskb->ip_summed = CHECKSUM_NONE;
405 nskb->protocol = eth_type_trans(nskb, dev);
406 netif_rx_ni(nskb);
407 return 0;
408
409badframe:
410 dev->stats.rx_errors++;
411 kfree_skb(skb);
412 return 0;
413}
414
415static u8 __bnep_tx_types[] = {
416 BNEP_GENERAL,
417 BNEP_COMPRESSED_SRC_ONLY,
418 BNEP_COMPRESSED_DST_ONLY,
419 BNEP_COMPRESSED
420};
421
422static int bnep_tx_frame(struct bnep_session *s, struct sk_buff *skb)
423{
424 struct ethhdr *eh = (void *) skb->data;
425 struct socket *sock = s->sock;
426 struct kvec iv[3];
427 int len = 0, il = 0;
428 u8 type = 0;
429
430 BT_DBG("skb %p dev %p type %d", skb, skb->dev, skb->pkt_type);
431
432 if (!skb->dev) {
433
434 goto send;
435 }
436
437 iv[il++] = (struct kvec) { &type, 1 };
438 len++;
439
440 if (compress_src && ether_addr_equal(eh->h_dest, s->eh.h_source))
441 type |= 0x01;
442
443 if (compress_dst && ether_addr_equal(eh->h_source, s->eh.h_dest))
444 type |= 0x02;
445
446 if (type)
447 skb_pull(skb, ETH_ALEN * 2);
448
449 type = __bnep_tx_types[type];
450 switch (type) {
451 case BNEP_COMPRESSED_SRC_ONLY:
452 iv[il++] = (struct kvec) { eh->h_source, ETH_ALEN };
453 len += ETH_ALEN;
454 break;
455
456 case BNEP_COMPRESSED_DST_ONLY:
457 iv[il++] = (struct kvec) { eh->h_dest, ETH_ALEN };
458 len += ETH_ALEN;
459 break;
460 }
461
462send:
463 iv[il++] = (struct kvec) { skb->data, skb->len };
464 len += skb->len;
465
466
467 {
468 len = kernel_sendmsg(sock, &s->msg, iv, il, len);
469 }
470 kfree_skb(skb);
471
472 if (len > 0) {
473 s->dev->stats.tx_bytes += len;
474 s->dev->stats.tx_packets++;
475 return 0;
476 }
477
478 return len;
479}
480
481static int bnep_session(void *arg)
482{
483 struct bnep_session *s = arg;
484 struct net_device *dev = s->dev;
485 struct sock *sk = s->sock->sk;
486 struct sk_buff *skb;
487 wait_queue_t wait;
488
489 BT_DBG("");
490
491 set_user_nice(current, -15);
492
493 init_waitqueue_entry(&wait, current);
494 add_wait_queue(sk_sleep(sk), &wait);
495 while (1) {
496 set_current_state(TASK_INTERRUPTIBLE);
497
498 if (atomic_read(&s->terminate))
499 break;
500
501 while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
502 skb_orphan(skb);
503 if (!skb_linearize(skb))
504 bnep_rx_frame(s, skb);
505 else
506 kfree_skb(skb);
507 }
508
509 if (sk->sk_state != BT_CONNECTED)
510 break;
511
512
513 while ((skb = skb_dequeue(&sk->sk_write_queue)))
514 if (bnep_tx_frame(s, skb))
515 break;
516 netif_wake_queue(dev);
517
518 schedule();
519 }
520 __set_current_state(TASK_RUNNING);
521 remove_wait_queue(sk_sleep(sk), &wait);
522
523
524 down_write(&bnep_session_sem);
525
526
527 unregister_netdev(dev);
528
529
530 s->sock->sk->sk_err = EUNATCH;
531
532 wake_up_interruptible(sk_sleep(s->sock->sk));
533
534
535 fput(s->sock->file);
536
537 __bnep_unlink_session(s);
538
539 up_write(&bnep_session_sem);
540 free_netdev(dev);
541 module_put_and_exit(0);
542 return 0;
543}
544
545static struct device *bnep_get_device(struct bnep_session *session)
546{
547 struct l2cap_conn *conn = l2cap_pi(session->sock->sk)->chan->conn;
548
549 if (!conn || !conn->hcon)
550 return NULL;
551
552 return &conn->hcon->dev;
553}
554
555static struct device_type bnep_type = {
556 .name = "bluetooth",
557};
558
559int bnep_add_connection(struct bnep_connadd_req *req, struct socket *sock)
560{
561 u32 valid_flags = BIT(BNEP_SETUP_RESPONSE);
562 struct net_device *dev;
563 struct bnep_session *s, *ss;
564 u8 dst[ETH_ALEN], src[ETH_ALEN];
565 int err;
566
567 BT_DBG("");
568
569 if (!l2cap_is_socket(sock))
570 return -EBADFD;
571
572 if (req->flags & ~valid_flags)
573 return -EINVAL;
574
575 baswap((void *) dst, &l2cap_pi(sock->sk)->chan->dst);
576 baswap((void *) src, &l2cap_pi(sock->sk)->chan->src);
577
578
579 dev = alloc_netdev(sizeof(struct bnep_session),
580 (*req->device) ? req->device : "bnep%d",
581 bnep_net_setup);
582 if (!dev)
583 return -ENOMEM;
584
585 down_write(&bnep_session_sem);
586
587 ss = __bnep_get_session(dst);
588 if (ss && ss->state == BT_CONNECTED) {
589 err = -EEXIST;
590 goto failed;
591 }
592
593 s = netdev_priv(dev);
594
595
596
597 memcpy(s->eh.h_dest, &src, ETH_ALEN);
598 memcpy(s->eh.h_source, &dst, ETH_ALEN);
599 memcpy(dev->dev_addr, s->eh.h_dest, ETH_ALEN);
600
601 s->dev = dev;
602 s->sock = sock;
603 s->role = req->role;
604 s->state = BT_CONNECTED;
605 s->flags = req->flags;
606
607 s->msg.msg_flags = MSG_NOSIGNAL;
608
609#ifdef CONFIG_BT_BNEP_MC_FILTER
610
611
612
613
614 s->mc_filter = ~0LL;
615#endif
616
617#ifdef CONFIG_BT_BNEP_PROTO_FILTER
618
619 bnep_set_default_proto_filter(s);
620#endif
621
622 SET_NETDEV_DEV(dev, bnep_get_device(s));
623 SET_NETDEV_DEVTYPE(dev, &bnep_type);
624
625 err = register_netdev(dev);
626 if (err)
627 goto failed;
628
629 __bnep_link_session(s);
630
631 __module_get(THIS_MODULE);
632 s->task = kthread_run(bnep_session, s, "kbnepd %s", dev->name);
633 if (IS_ERR(s->task)) {
634
635 module_put(THIS_MODULE);
636 unregister_netdev(dev);
637 __bnep_unlink_session(s);
638 err = PTR_ERR(s->task);
639 goto failed;
640 }
641
642 up_write(&bnep_session_sem);
643 strcpy(req->device, dev->name);
644 return 0;
645
646failed:
647 up_write(&bnep_session_sem);
648 free_netdev(dev);
649 return err;
650}
651
652int bnep_del_connection(struct bnep_conndel_req *req)
653{
654 u32 valid_flags = 0;
655 struct bnep_session *s;
656 int err = 0;
657
658 BT_DBG("");
659
660 if (req->flags & ~valid_flags)
661 return -EINVAL;
662
663 down_read(&bnep_session_sem);
664
665 s = __bnep_get_session(req->dst);
666 if (s) {
667 atomic_inc(&s->terminate);
668 wake_up_process(s->task);
669 } else
670 err = -ENOENT;
671
672 up_read(&bnep_session_sem);
673 return err;
674}
675
676static void __bnep_copy_ci(struct bnep_conninfo *ci, struct bnep_session *s)
677{
678 u32 valid_flags = BIT(BNEP_SETUP_RESPONSE);
679
680 memset(ci, 0, sizeof(*ci));
681 memcpy(ci->dst, s->eh.h_source, ETH_ALEN);
682 strcpy(ci->device, s->dev->name);
683 ci->flags = s->flags & valid_flags;
684 ci->state = s->state;
685 ci->role = s->role;
686}
687
688int bnep_get_connlist(struct bnep_connlist_req *req)
689{
690 struct bnep_session *s;
691 int err = 0, n = 0;
692
693 down_read(&bnep_session_sem);
694
695 list_for_each_entry(s, &bnep_session_list, list) {
696 struct bnep_conninfo ci;
697
698 __bnep_copy_ci(&ci, s);
699
700 if (copy_to_user(req->ci, &ci, sizeof(ci))) {
701 err = -EFAULT;
702 break;
703 }
704
705 if (++n >= req->cnum)
706 break;
707
708 req->ci++;
709 }
710 req->cnum = n;
711
712 up_read(&bnep_session_sem);
713 return err;
714}
715
716int bnep_get_conninfo(struct bnep_conninfo *ci)
717{
718 struct bnep_session *s;
719 int err = 0;
720
721 down_read(&bnep_session_sem);
722
723 s = __bnep_get_session(ci->dst);
724 if (s)
725 __bnep_copy_ci(ci, s);
726 else
727 err = -ENOENT;
728
729 up_read(&bnep_session_sem);
730 return err;
731}
732
733static int __init bnep_init(void)
734{
735 char flt[50] = "";
736
737#ifdef CONFIG_BT_BNEP_PROTO_FILTER
738 strcat(flt, "protocol ");
739#endif
740
741#ifdef CONFIG_BT_BNEP_MC_FILTER
742 strcat(flt, "multicast");
743#endif
744
745 BT_INFO("BNEP (Ethernet Emulation) ver %s", VERSION);
746 if (flt[0])
747 BT_INFO("BNEP filters: %s", flt);
748
749 bnep_sock_init();
750 return 0;
751}
752
753static void __exit bnep_exit(void)
754{
755 bnep_sock_cleanup();
756}
757
758module_init(bnep_init);
759module_exit(bnep_exit);
760
761module_param(compress_src, bool, 0644);
762MODULE_PARM_DESC(compress_src, "Compress sources headers");
763
764module_param(compress_dst, bool, 0644);
765MODULE_PARM_DESC(compress_dst, "Compress destination headers");
766
767MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
768MODULE_DESCRIPTION("Bluetooth BNEP ver " VERSION);
769MODULE_VERSION(VERSION);
770MODULE_LICENSE("GPL");
771MODULE_ALIAS("bt-proto-4");
772
