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