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