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