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