1 /*
2 BNEP implementation for Linux Bluetooth stack (BlueZ).
3 Copyright (C) 2001-2002 Inventel Systemes
4 Written 2001-2002 by
5 Clément Moreau <clement.moreau@inventel.fr>
6 David Libault <david.libault@inventel.fr>
7
8 Copyright (C) 2002 Maxim Krasnyansky <maxk@qualcomm.com>
9
10 This program is free software; you can redistribute it and/or modify
11 it under the terms of the GNU General Public License version 2 as
12 published by the Free Software Foundation;
13
14 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
15 OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
16 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS.
17 IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY
18 CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES
19 WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
20 ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
21 OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
22
23 ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS,
24 COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS
25 SOFTWARE IS DISCLAIMED.
26 */
27
28 /*
29 * $Id: core.c,v 1.20 2002/08/04 21:23:58 maxk Exp $
30 */
31
32 #include <linux/config.h>
33 #include <linux/module.h>
34
35 #include <linux/kernel.h>
36 #include <linux/sched.h>
37 #include <linux/signal.h>
38 #include <linux/init.h>
39 #include <linux/wait.h>
40 #include <linux/errno.h>
41 #include <linux/smp_lock.h>
42 #include <linux/net.h>
43 #include <net/sock.h>
44
45 #include <linux/socket.h>
46 #include <linux/file.h>
47
48 #include <linux/netdevice.h>
49 #include <linux/etherdevice.h>
50 #include <linux/skbuff.h>
51
52 #include <asm/unaligned.h>
53
54 #include <net/bluetooth/bluetooth.h>
55 #include <net/bluetooth/l2cap.h>
56
57 #include "bnep.h"
58
59 #ifndef CONFIG_BT_BNEP_DEBUG
60 #undef BT_DBG
61 #define BT_DBG(D...)
62 #endif
63
64 #define VERSION "1.2"
65
66 static LIST_HEAD(bnep_session_list);
67 static DECLARE_RWSEM(bnep_session_sem);
68
69 static struct bnep_session *__bnep_get_session(u8 *dst)
70 {
71 struct bnep_session *s;
72 struct list_head *p;
73
74 BT_DBG("");
75
76 list_for_each(p, &bnep_session_list) {
77 s = list_entry(p, struct bnep_session, list);
78 if (!memcmp(dst, s->eh.h_source, ETH_ALEN))
79 return s;
80 }
81 return NULL;
82 }
83
84 static void __bnep_link_session(struct bnep_session *s)
85 {
86 /* It's safe to call __module_get() here because sessions are added
87 by the socket layer which has to hold the refference to this module.
88 */
89 __module_get(THIS_MODULE);
90 list_add(&s->list, &bnep_session_list);
91 }
92
93 static void __bnep_unlink_session(struct bnep_session *s)
94 {
95 list_del(&s->list);
96 module_put(THIS_MODULE);
97 }
98
99 static int bnep_send(struct bnep_session *s, void *data, size_t len)
100 {
101 struct socket *sock = s->sock;
102 struct kvec iv = { data, len };
103
104 return kernel_sendmsg(sock, &s->msg, &iv, 1, len);
105 }
106
107 static int bnep_send_rsp(struct bnep_session *s, u8 ctrl, u16 resp)
108 {
109 struct bnep_control_rsp rsp;
110 rsp.type = BNEP_CONTROL;
111 rsp.ctrl = ctrl;
112 rsp.resp = htons(resp);
113 return bnep_send(s, &rsp, sizeof(rsp));
114 }
115
116 #ifdef CONFIG_BT_BNEP_PROTO_FILTER
117 static inline void bnep_set_default_proto_filter(struct bnep_session *s)
118 {
119 /* (IPv4, ARP) */
120 s->proto_filter[0].start = htons(0x0800);
121 s->proto_filter[0].end = htons(0x0806);
122 /* (RARP, AppleTalk) */
123 s->proto_filter[1].start = htons(0x8035);
124 s->proto_filter[1].end = htons(0x80F3);
125 /* (IPX, IPv6) */
126 s->proto_filter[2].start = htons(0x8137);
127 s->proto_filter[2].end = htons(0x86DD);
128 }
129 #endif
130
131 static int bnep_ctrl_set_netfilter(struct bnep_session *s, u16 *data, int len)
132 {
133 int n;
134
135 if (len < 2)
136 return -EILSEQ;
137
138 n = ntohs(get_unaligned(data));
139 data++; len -= 2;
140
141 if (len < n)
142 return -EILSEQ;
143
144 BT_DBG("filter len %d", n);
145
146 #ifdef CONFIG_BT_BNEP_PROTO_FILTER
147 n /= 4;
148 if (n <= BNEP_MAX_PROTO_FILTERS) {
149 struct bnep_proto_filter *f = s->proto_filter;
150 int i;
151
152 for (i = 0; i < n; i++) {
153 f[i].start = get_unaligned(data++);
154 f[i].end = get_unaligned(data++);
155
156 BT_DBG("proto filter start %d end %d",
157 f[i].start, f[i].end);
158 }
159
160 if (i < BNEP_MAX_PROTO_FILTERS)
161 memset(f + i, 0, sizeof(*f));
162
163 if (n == 0)
164 bnep_set_default_proto_filter(s);
165
166 bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_SUCCESS);
167 } else {
168 bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_FILTER_LIMIT_REACHED);
169 }
170 #else
171 bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_FILTER_UNSUPPORTED_REQ);
172 #endif
173 return 0;
174 }
175
176 static int bnep_ctrl_set_mcfilter(struct bnep_session *s, u8 *data, int len)
177 {
178 int n;
179
180 if (len < 2)
181 return -EILSEQ;
182
183 n = ntohs(get_unaligned((u16 *) data));
184 data += 2; len -= 2;
185
186 if (len < n)
187 return -EILSEQ;
188
189 BT_DBG("filter len %d", n);
190
191 #ifdef CONFIG_BT_BNEP_MC_FILTER
192 n /= (ETH_ALEN * 2);
193
194 if (n > 0) {
195 s->mc_filter = 0;
196
197 /* Always send broadcast */
198 set_bit(bnep_mc_hash(s->dev->broadcast), (ulong *) &s->mc_filter);
199
200 /* Add address ranges to the multicast hash */
201 for (; n > 0; n--) {
202 u8 a1[6], *a2;
203
204 memcpy(a1, data, ETH_ALEN); data += ETH_ALEN;
205 a2 = data; data += ETH_ALEN;
206
207 BT_DBG("mc filter %s -> %s",
208 batostr((void *) a1), batostr((void *) a2));
209
210 #define INCA(a) { int i = 5; while (i >=0 && ++a[i--] == 0); }
211
212 /* Iterate from a1 to a2 */
213 set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter);
214 while (memcmp(a1, a2, 6) < 0 && s->mc_filter != ~0LL) {
215 INCA(a1);
216 set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter);
217 }
218 }
219 }
220
221 BT_DBG("mc filter hash 0x%llx", s->mc_filter);
222
223 bnep_send_rsp(s, BNEP_FILTER_MULTI_ADDR_RSP, BNEP_SUCCESS);
224 #else
225 bnep_send_rsp(s, BNEP_FILTER_MULTI_ADDR_RSP, BNEP_FILTER_UNSUPPORTED_REQ);
226 #endif
227 return 0;
228 }
229
230 static int bnep_rx_control(struct bnep_session *s, void *data, int len)
231 {
232 u8 cmd = *(u8 *)data;
233 int err = 0;
234
235 data++; len--;
236
237 switch (cmd) {
238 case BNEP_CMD_NOT_UNDERSTOOD:
239 case BNEP_SETUP_CONN_REQ:
240 case BNEP_SETUP_CONN_RSP:
241 case BNEP_FILTER_NET_TYPE_RSP:
242 case BNEP_FILTER_MULTI_ADDR_RSP:
243 /* Ignore these for now */
244 break;
245
246 case BNEP_FILTER_NET_TYPE_SET:
247 err = bnep_ctrl_set_netfilter(s, data, len);
248 break;
249
250 case BNEP_FILTER_MULTI_ADDR_SET:
251 err = bnep_ctrl_set_mcfilter(s, data, len);
252 break;
253
254 default: {
255 u8 pkt[3];
256 pkt[0] = BNEP_CONTROL;
257 pkt[1] = BNEP_CMD_NOT_UNDERSTOOD;
258 pkt[2] = cmd;
259 bnep_send(s, pkt, sizeof(pkt));
260 }
261 break;
262 }
263
264 return err;
265 }
266
267 static int bnep_rx_extension(struct bnep_session *s, struct sk_buff *skb)
268 {
269 struct bnep_ext_hdr *h;
270 int err = 0;
271
272 do {
273 h = (void *) skb->data;
274 if (!skb_pull(skb, sizeof(*h))) {
275 err = -EILSEQ;
276 break;
277 }
278
279 BT_DBG("type 0x%x len %d", h->type, h->len);
280
281 switch (h->type & BNEP_TYPE_MASK) {
282 case BNEP_EXT_CONTROL:
283 bnep_rx_control(s, skb->data, skb->len);
284 break;
285
286 default:
287 /* Unknown extension, skip it. */
288 break;
289 }
290
291 if (!skb_pull(skb, h->len)) {
292 err = -EILSEQ;
293 break;
294 }
295 } while (!err && (h->type & BNEP_EXT_HEADER));
296
297 return err;
298 }
299
300 static u8 __bnep_rx_hlen[] = {
301 ETH_HLEN, /* BNEP_GENERAL */
302 0, /* BNEP_CONTROL */
303 2, /* BNEP_COMPRESSED */
304 ETH_ALEN + 2, /* BNEP_COMPRESSED_SRC_ONLY */
305 ETH_ALEN + 2 /* BNEP_COMPRESSED_DST_ONLY */
306 };
307 #define BNEP_RX_TYPES (sizeof(__bnep_rx_hlen) - 1)
308
309 static inline int bnep_rx_frame(struct bnep_session *s, struct sk_buff *skb)
310 {
311 struct net_device *dev = s->dev;
312 struct sk_buff *nskb;
313 u8 type;
314
315 dev->last_rx = jiffies;
316 s->stats.rx_bytes += skb->len;
317
318 type = *(u8 *) skb->data; skb_pull(skb, 1);
319
320 if ((type & BNEP_TYPE_MASK) > BNEP_RX_TYPES)
321 goto badframe;
322
323 if ((type & BNEP_TYPE_MASK) == BNEP_CONTROL) {
324 bnep_rx_control(s, skb->data, skb->len);
325 kfree_skb(skb);
326 return 0;
327 }
328
329 skb->mac.raw = skb->data;
330
331 /* Verify and pull out header */
332 if (!skb_pull(skb, __bnep_rx_hlen[type & BNEP_TYPE_MASK]))
333 goto badframe;
334
335 s->eh.h_proto = get_unaligned((u16 *) (skb->data - 2));
336
337 if (type & BNEP_EXT_HEADER) {
338 if (bnep_rx_extension(s, skb) < 0)
339 goto badframe;
340 }
341
342 /* Strip 802.1p header */
343 if (ntohs(s->eh.h_proto) == 0x8100) {
344 if (!skb_pull(skb, 4))
345 goto badframe;
346 s->eh.h_proto = get_unaligned((u16 *) (skb->data - 2));
347 }
348
349 /* We have to alloc new skb and copy data here :(. Because original skb
350 * may not be modified and because of the alignment requirements. */
351 nskb = alloc_skb(2 + ETH_HLEN + skb->len, GFP_KERNEL);
352 if (!nskb) {
353 s->stats.rx_dropped++;
354 kfree_skb(skb);
355 return -ENOMEM;
356 }
357 skb_reserve(nskb, 2);
358
359 /* Decompress header and construct ether frame */
360 switch (type & BNEP_TYPE_MASK) {
361 case BNEP_COMPRESSED:
362 memcpy(__skb_put(nskb, ETH_HLEN), &s->eh, ETH_HLEN);
363 break;
364
365 case BNEP_COMPRESSED_SRC_ONLY:
366 memcpy(__skb_put(nskb, ETH_ALEN), s->eh.h_dest, ETH_ALEN);
367 memcpy(__skb_put(nskb, ETH_ALEN), skb->mac.raw, ETH_ALEN);
368 put_unaligned(s->eh.h_proto, (u16 *) __skb_put(nskb, 2));
369 break;
370
371 case BNEP_COMPRESSED_DST_ONLY:
372 memcpy(__skb_put(nskb, ETH_ALEN), skb->mac.raw, ETH_ALEN);
373 memcpy(__skb_put(nskb, ETH_ALEN + 2), s->eh.h_source, ETH_ALEN + 2);
374 break;
375
376 case BNEP_GENERAL:
377 memcpy(__skb_put(nskb, ETH_ALEN * 2), skb->mac.raw, ETH_ALEN * 2);
378 put_unaligned(s->eh.h_proto, (u16 *) __skb_put(nskb, 2));
379 break;
380 }
381
382 memcpy(__skb_put(nskb, skb->len), skb->data, skb->len);
383 kfree_skb(skb);
384
385 s->stats.rx_packets++;
386 nskb->dev = dev;
387 nskb->ip_summed = CHECKSUM_NONE;
388 nskb->protocol = eth_type_trans(nskb, dev);
389 netif_rx_ni(nskb);
390 return 0;
391
392 badframe:
393 s->stats.rx_errors++;
394 kfree_skb(skb);
395 return 0;
396 }
397
398 static u8 __bnep_tx_types[] = {
399 BNEP_GENERAL,
400 BNEP_COMPRESSED_SRC_ONLY,
401 BNEP_COMPRESSED_DST_ONLY,
402 BNEP_COMPRESSED
403 };
404
405 static inline int bnep_tx_frame(struct bnep_session *s, struct sk_buff *skb)
406 {
407 struct ethhdr *eh = (void *) skb->data;
408 struct socket *sock = s->sock;
409 struct kvec iv[3];
410 int len = 0, il = 0;
411 u8 type = 0;
412
413 BT_DBG("skb %p dev %p type %d", skb, skb->dev, skb->pkt_type);
414
415 if (!skb->dev) {
416 /* Control frame sent by us */
417 goto send;
418 }
419
420 iv[il++] = (struct kvec) { &type, 1 };
421 len++;
422
423 if (!memcmp(eh->h_dest, s->eh.h_source, ETH_ALEN))
424 type |= 0x01;
425
426 if (!memcmp(eh->h_source, s->eh.h_dest, ETH_ALEN))
427 type |= 0x02;
428
429 if (type)
430 skb_pull(skb, ETH_ALEN * 2);
431
432 type = __bnep_tx_types[type];
433 switch (type) {
434 case BNEP_COMPRESSED_SRC_ONLY:
435 iv[il++] = (struct kvec) { eh->h_source, ETH_ALEN };
436 len += ETH_ALEN;
437 break;
438
439 case BNEP_COMPRESSED_DST_ONLY:
440 iv[il++] = (struct kvec) { eh->h_dest, ETH_ALEN };
441 len += ETH_ALEN;
442 break;
443 }
444
445 send:
446 iv[il++] = (struct kvec) { skb->data, skb->len };
447 len += skb->len;
448
449 /* FIXME: linearize skb */
450 {
451 len = kernel_sendmsg(sock, &s->msg, iv, il, len);
452 }
453 kfree_skb(skb);
454
455 if (len > 0) {
456 s->stats.tx_bytes += len;
457 s->stats.tx_packets++;
458 return 0;
459 }
460
461 return len;
462 }
463
464 static int bnep_session(void *arg)
465 {
466 struct bnep_session *s = arg;
467 struct net_device *dev = s->dev;
468 struct sock *sk = s->sock->sk;
469 struct sk_buff *skb;
470 wait_queue_t wait;
471
472 BT_DBG("");
473
474 daemonize("kbnepd %s", dev->name);
475 set_user_nice(current, -15);
476 current->flags |= PF_NOFREEZE;
477
478 init_waitqueue_entry(&wait, current);
479 add_wait_queue(sk->sk_sleep, &wait);
480 while (!atomic_read(&s->killed)) {
481 set_current_state(TASK_INTERRUPTIBLE);
482
483 // RX
484 while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
485 skb_orphan(skb);
486 bnep_rx_frame(s, skb);
487 }
488
489 if (sk->sk_state != BT_CONNECTED)
490 break;
491
492 // TX
493 while ((skb = skb_dequeue(&sk->sk_write_queue)))
494 if (bnep_tx_frame(s, skb))
495 break;
496 netif_wake_queue(dev);
497
498 schedule();
499 }
500 set_current_state(TASK_RUNNING);
501 remove_wait_queue(sk->sk_sleep, &wait);
502
503 /* Cleanup session */
504 down_write(&bnep_session_sem);
505
506 /* Delete network device */
507 unregister_netdev(dev);
508
509 /* Release the socket */
510 fput(s->sock->file);
511
512 __bnep_unlink_session(s);
513
514 up_write(&bnep_session_sem);
515 free_netdev(dev);
516 return 0;
517 }
518
519 int bnep_add_connection(struct bnep_connadd_req *req, struct socket *sock)
520 {
521 struct net_device *dev;
522 struct bnep_session *s, *ss;
523 u8 dst[ETH_ALEN], src[ETH_ALEN];
524 int err;
525
526 BT_DBG("");
527
528 baswap((void *) dst, &bt_sk(sock->sk)->dst);
529 baswap((void *) src, &bt_sk(sock->sk)->src);
530
531 /* session struct allocated as private part of net_device */
532 dev = alloc_netdev(sizeof(struct bnep_session),
533 (*req->device) ? req->device : "bnep%d",
534 bnep_net_setup);
535 if (!dev)
536 return ENOMEM;
537
538
539 down_write(&bnep_session_sem);
540
541 ss = __bnep_get_session(dst);
542 if (ss && ss->state == BT_CONNECTED) {
543 err = -EEXIST;
544 goto failed;
545 }
546
547 s = dev->priv;
548
549 /* This is rx header therefore addresses are swapped.
550 * ie eh.h_dest is our local address. */
551 memcpy(s->eh.h_dest, &src, ETH_ALEN);
552 memcpy(s->eh.h_source, &dst, ETH_ALEN);
553 memcpy(dev->dev_addr, s->eh.h_dest, ETH_ALEN);
554
555 s->dev = dev;
556 s->sock = sock;
557 s->role = req->role;
558 s->state = BT_CONNECTED;
559
560 s->msg.msg_flags = MSG_NOSIGNAL;
561
562 #ifdef CONFIG_BT_BNEP_MC_FILTER
563 /* Set default mc filter */
564 set_bit(bnep_mc_hash(dev->broadcast), (ulong *) &s->mc_filter);
565 #endif
566
567 #ifdef CONFIG_BT_BNEP_PROTO_FILTER
568 /* Set default protocol filter */
569 bnep_set_default_proto_filter(s);
570 #endif
571
572 err = register_netdev(dev);
573 if (err) {
574 goto failed;
575 }
576
577 __bnep_link_session(s);
578
579 err = kernel_thread(bnep_session, s, CLONE_KERNEL);
580 if (err < 0) {
581 /* Session thread start failed, gotta cleanup. */
582 unregister_netdev(dev);
583 __bnep_unlink_session(s);
584 goto failed;
585 }
586
587 up_write(&bnep_session_sem);
588 strcpy(req->device, dev->name);
589 return 0;
590
591 failed:
592 up_write(&bnep_session_sem);
593 free_netdev(dev);
594 return err;
595 }
596
597 int bnep_del_connection(struct bnep_conndel_req *req)
598 {
599 struct bnep_session *s;
600 int err = 0;
601
602 BT_DBG("");
603
604 down_read(&bnep_session_sem);
605
606 s = __bnep_get_session(req->dst);
607 if (s) {
608 /* Wakeup user-space which is polling for socket errors.
609 * This is temporary hack untill we have shutdown in L2CAP */
610 s->sock->sk->sk_err = EUNATCH;
611
612 /* Kill session thread */
613 atomic_inc(&s->killed);
614 wake_up_interruptible(s->sock->sk->sk_sleep);
615 } else
616 err = -ENOENT;
617
618 up_read(&bnep_session_sem);
619 return err;
620 }
621
622 static void __bnep_copy_ci(struct bnep_conninfo *ci, struct bnep_session *s)
623 {
624 memcpy(ci->dst, s->eh.h_source, ETH_ALEN);
625 strcpy(ci->device, s->dev->name);
626 ci->flags = s->flags;
627 ci->state = s->state;
628 ci->role = s->role;
629 }
630
631 int bnep_get_connlist(struct bnep_connlist_req *req)
632 {
633 struct list_head *p;
634 int err = 0, n = 0;
635
636 down_read(&bnep_session_sem);
637
638 list_for_each(p, &bnep_session_list) {
639 struct bnep_session *s;
640 struct bnep_conninfo ci;
641
642 s = list_entry(p, struct bnep_session, list);
643
644 __bnep_copy_ci(&ci, s);
645
646 if (copy_to_user(req->ci, &ci, sizeof(ci))) {
647 err = -EFAULT;
648 break;
649 }
650
651 if (++n >= req->cnum)
652 break;
653
654 req->ci++;
655 }
656 req->cnum = n;
657
658 up_read(&bnep_session_sem);
659 return err;
660 }
661
662 int bnep_get_conninfo(struct bnep_conninfo *ci)
663 {
664 struct bnep_session *s;
665 int err = 0;
666
667 down_read(&bnep_session_sem);
668
669 s = __bnep_get_session(ci->dst);
670 if (s)
671 __bnep_copy_ci(ci, s);
672 else
673 err = -ENOENT;
674
675 up_read(&bnep_session_sem);
676 return err;
677 }
678
679 static int __init bnep_init(void)
680 {
681 char flt[50] = "";
682
683 l2cap_load();
684
685 #ifdef CONFIG_BT_BNEP_PROTO_FILTER
686 strcat(flt, "protocol ");
687 #endif
688
689 #ifdef CONFIG_BT_BNEP_MC_FILTER
690 strcat(flt, "multicast");
691 #endif
692
693 BT_INFO("BNEP (Ethernet Emulation) ver %s", VERSION);
694 if (flt[0])
695 BT_INFO("BNEP filters: %s", flt);
696
697 bnep_sock_init();
698 return 0;
699 }
700
701 static void __exit bnep_exit(void)
702 {
703 bnep_sock_cleanup();
704 }
705
706 module_init(bnep_init);
707 module_exit(bnep_exit);
708
709 MODULE_AUTHOR("David Libault <david.libault@inventel.fr>, Maxim Krasnyansky <maxk@qualcomm.com>");
710 MODULE_DESCRIPTION("Bluetooth BNEP ver " VERSION);
711 MODULE_VERSION(VERSION);
712 MODULE_LICENSE("GPL");
713 MODULE_ALIAS("bt-proto-4");
714
|
This page was automatically generated by the
LXR engine.
|