Merge branch 'master' of /home/aia21/ntfs-2.6/
[linux-2.6] / net / bluetooth / bnep / core.c
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 (!compare_ether_addr(dst, s->eh.h_source))
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 (!compare_ether_addr(eh->h_dest, s->eh.h_source))
424                 type |= 0x01;
425
426         if (!compare_ether_addr(eh->h_source, s->eh.h_dest))
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");